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ABSTRACT 


At the Naval Postgraduate School (NPS), a small AUV navigation system 
(SANS) was developed for research in support of shallow-water mine countermeasures 
and coastal environmental monitoring. The objective of this thesis is to test and evaluate 
the SANS performance after tuning the filter gains through a series of testing procedures. 

The new version of SANS (SANS III) used new hardware components which 
were smaller, cheaper, and more reliable. A PC/104 computer provided more computing 
power and, increased the reliability and compatibility of the system. 

Implementing an asynchronous Kalman filter in the position and velocity 
estimation part of the navigation subsystem improved the navigation accuracy 
significantly. To determine and evaluate the overall system performance, ground vehicle 
testing was conducted. Test results showed that the SANS III was able to navigate within 
±15 feet of Global Positioning track with no Global Positioning update for three 
minutes. 
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I. INTRODUCTION 


A. BACKGROUND 

Autonomous Underwater Vehicles (AUVs) are capable of a variety of overt and 
clandestine missions. Such vehicles have been used for inspection, mine 
countermeasures, survey, and observation [Ref. 1]. One of the most important and 
difficult aspects of an AUV mission is navigation. Due to intermittent submergence, the 
vehicle does not have continuous access to external navigational aids such as the Global 
Positioning System (GPS) or Loran. Thus the vehicle itself must be capable of 
estimating the current position from onboard sensors that measure speed, heading, 
velocity and acceleration. Previous studies have shown that integrating GPS and INS 
into a single system makes possible production of continuously accurate navigational 
information even when using relatively low-cost components [Ref. 11]. GPS is capable 
of supplying accurate positioning when the AUV is surfaced. It is integrated with an INS 
to compensate for the loss of GPS signals when the AUV is submerged. 

At the Naval Postgraduate School (NPS), a Small AUV Navigation System 
(SANS) was developed for research in support of shallow-water mine countermeasures 
and coastal environmental monitoring. The goal was to demonstrate the feasibility of 
using a small, low-cost Inertial Measurement Unit (IMU) to navigate between 
Differential Global Positioning System (DGPS) fixes. 

The first prototype of the SANS system (called SANS I) was separated into two 

subsystems [Ref. 12]. The IMU, water speed sensor, compass, GPS antenna, and data 

logging computer were housed in one package and placed in a towfish. The GPS 
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receiver, DGPS antenna, and data processing computer were in the towing vessel. The 
data collected by the towfish subsystem were transmitted to the processing computer 
through a modem cable. 

The second generation of the SANS system, or SANS II, was totally contained in 
a single package. The software of SANS I and SANS II was based on a twelve-state 
constant gain complementary filter. The values of these gains were initially selected 
based on bandwidth considerations, and later tuned based on results from bench testing 
and ground vehicle testing. 

SANS III, the current version of SANS, is composed of the state-of-the-art 
components, which include an IMU, GPS/DGPS Receiver, magnetic compass, water 
speed sensor, and data processing computer. The 486 based ESP computer used in the 
previous versions of SANS was replaced by an AMD 586DX133 based PC/104 computer 
to provide more computing power, to increase reliability and provide compatibility with 
PC/104 industrial standards [Ref. 4]. 

In the current version of SANS, the position and velocity estimation part of the 
constant-gain filter used in SANS I and SANS II is replaced by an asynchronous Kalman 
filter. Kalman filtering is a method of combining all available sensor data, regardless of 
their precision, to estimate the current position of a vehicle. Use of a Kalman filter 
improves performance, decreases navigational errors, and improves reliability relative to 
filters that depend only on one sensor input. 

B. RESEARCH OBJECTIVES 

The objectives of this thesis research are the following: 

- to implement Kalman filtering within the SANS III, 
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- to improve navigation accuracy of the SANS III by properly tuning filter gains, 
-to test and evaluate the overall performance of the SANS III through ground 
vehicle testing in preparation for the at-sea testing. 

C. SCOPE, LIMITATIONS, AND ASSUMPTIONS 

This thesis reports part of the findings of more than eight years of research in an 
ongoing project. The main objective of this thesis is to test and. evaluate the current 
configuration of the SANS through bench and ground vehicle tests, and tune the Kalman 
filter gains to increase reliability and accuracy of the system before at-sea testing. 

D. ORGANIZATION OF THESIS 

Chapter II provides an overview and description of the current hardware and 
software components of the system. 

Chapter III describes the Kalman filter for the SANS III. 

Chapter IV describes system test procedures and results. 

Chapter V presents the thesis conclusions and provides recommendations for 
future research. 
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II. SYSTEM OVERVIEW 


A. INTRODUCTION 

References [13 and 14] include detailed information about the current SANS 
hardware and software components. The purpose of this chapter is to present an 
overview of the configuration of SANS III hardware and software units. 

B. SANS III HARDWARE OVERVIEW 

Most of the previous SANS hardware parts have been replaced with more 
powerful, flexible, and reliable components which are faster, smaller, and cheaper. 
Figure 2.1 shows the SANS III hardware configuration. The main components are a Real 
Time Devices 133 MHz AMD 586 PC/104 module, a Sealevel C4-104 four-port PC/104 
compatible RS-232 module, a Real Time Devices DM406/DM5406 analog to digital 
(A/D) converter, a Real Time Devices CMT104 IDE hard drive module and data sensors 
which include the Crossbow DMU-VG Six Axis IMU, the Motorola Oncore/McKinney 
Technology GPS/DGPS Receiver unit, the Precision Navigation TCM2 Electronic 
Compass, and the B&G Sonic Water Speed Sensor. 

1. Real Time Devices AMD 586DX133 Based PC/104 

The Real Time Devices AMD 586DX133 based PC/104 is being used as a data 
acquisition and processing unit in SANS III system. PC/104 is an industrial standard for 
creating embedded computer applications. It fulfills the basic needs of embedded systems 
such as low power consumption, modularity, small foot print, high reliability, good noise 
immunity, high-speed operation, and expandability [Ref. 13]. 
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Figure 2.1 Current SANS Hardware Configuration [After Ref. 13] 

The PC/104 can be easily customized by stacking PC/104 modules that are 
compliant with the PC/104 bus architecture, such as video controllers, network interfaces, 
analog and digital data acquisition modules, sound I/O modules, etc [Ref. 4]. 

The SANS system is equipped with five PC/104 modules. These are the CPU 
module, the hard drive module, the VGA module, A/D converter and the serial I/O 
module. 

2. Sealevel C4-104 Serial I/O Module 

The PC/104 compatible Sealevel C4-104 serial I/O module provides four RS-232 
serial I/O ports for connecting the four sensors of SANS III system. Each serial port can 
be configured to have its own base memory address and Interrupt Request (IRQ) 
assignment. 
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The C4-104 is compliant with PC/104 specification regarding both mechanical 
and electrical specifications. The C4-104 utilizes Universal Asynchronous 
Receiver/Transmitters (UART) with programmable baud rates, data format, interrupt 
control and a 16-byte input and output FIFO [Ref. 7]. 

3. Real Time Devices DM406/DM5406 Analog to Digital (A/D) Converter 

The DM406/DM5406 A/D Converter can receive up to 8 differential or 16 single 

-ended analog inputs. It converts these inputs into 12-bit digital data words. The analog 
input voltage range is jumper-selectable for bipolar ranges of-5 to +5 volts, -10 to +10 
volts, or a unipolar range of 0 to +10 volts. The module is provided with overvoltage 
protection to + 35 volts. 

A/D conversions are performed in 5 microseconds, and the maximum throughput 
rate is 100 kHz. Conversions can be controlled through software, by an on-board pacer 
clock, or by an external trigger brought onto the board through the I/O connector. 

The converted data can be transferred to PC memory through the PC data bus or 
by using direct memory access (DMA). Detailed information for settings and 
programming the module is supplied in [Ref. 8], 

4. Crossbow DMU-VG Six Axis Inertial Measurement Unit 

The DMU-VG is a six-axis measurement system designed to measure linear 
acceleration along three orthogonal axes, and rotation rates around three orthogonal axes. 
It is designed to provide stabilized pitch and roll in dynamic environments [Ref. 10]. The 
IMU has both analog output and RS-232 serial port output. 

-The DMU-VG is designed to operate in one of two modes, a scaled sensor mode 
and a voltage mode. It was configured to operate in voltage mode for the SANS. 
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5. Motorola Oncore/McKinney Technology GPS/DGPS Receiver 

The GPS receiver unit used in the SANS system is based upon the Motorola 
ONCORE Receiver [Ref. 9]. It is capable of tracking eight satellites simultaneously. 
The GPS receiver incorporates a DGPS capability. The DGPS receiver unit was specially 
designed by McKinney Technology to operate in the Monterey Bay area. Its data port 
interface is RS-232 compatible. The output message consists of latitude, longitude, 
height, velocity, heading, time, and satellite tracking status. The test results conducted 
show that the GPS unit with differential corrections can provide a positional accuracy of 
15 ft. 

6. Precision Navigation TCM2 Electronic Compass 

The Precision Navigation model TCM2 Electronic Compass Module is used in 
the current SANS hardware configuration [Ref. 3]. The TCM2 consists of a three-axis 
magnetometer, a two-axis tilt sensor and a small A/D board. The tilt angle compensation 
of the sensor depends on the TCM2 model (20, 50, and 80 degrees). Output may include 
heading, roll, pitch, temperature, and a three dimensional magnetic field measurement. 
The TCM2 standard output format can be configured to provide those parameters 
required by the user. Precision Navigation literature lists accuracy as within one half of a 
degree in level operation. The TCM2 can be calibrated for local magnetic field 
distortions by the user. It provides an alarm in the output data when local magnetic 
anomalies, are present, and gives out-of-range warnings when the unit is being tilted too 
far. The calibration of the compass and its error characteristics are described in [Ref 3]. 
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7. B&G Microsomc Speed Sensor 

The B&G Water Speed Sensor (Figure 2.2) has an accuracy of 0.05 Knots. It can 
measure the speeds up to 30-40 Knots depending on the conditions of the signal path. 
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Figure 2.2 Water Speed Sensor 

The sensor transmits groups of ultrasonic pulses (short hurts of 500 kHz) from 
one transducer to the other, swapping transducers after each group. Each received pulse 
precisely triggers the next transmitted pulse. The total time taken for each group depends 
on the propagation time in the sonic path, hence variations in group time caused by water 


flow between the transducers can be measured and converted to the speed of water flow. 
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The microprocessor continually monitors the received pulses, adjusting the gain 
of the receiver in response to changes in propagation conditions (e.g. air bubbles) and 
also detects faulty sonic conditions. 

The size of the sensor is also appropriate for the concept of the SANS. It has an 
overall length of sixteen inches, and a height of three inches. 

C. SANS III SOFTWARE OVERVIEW 

The software of the SANS III was written in C++ and compiled using the Borland 
5.0. It is designed to utilize IMU, heading, and water-speed information to implement an 
INS based on an asynchronous Kalman filter. It also integrates GPS information with 
INS information to obtain continuously accurate navigation information in real time. The 
main flow between modules of the system is shown in Figure 2.3. 



Figure 2.3 SANS Software Main Modules 
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The modules, with the exception of the water speed sensor, take in the data 
through the serial ports and supply it to the main program, toefish, for screen set up and 
output. The Sealevel C4-104 module provides serial port connections for the devices. 
Water speed data is supplied to the system through the A/D converter. 

The GPS class object obtains GPS position messages over a RS-232 interface via 
the COM1 serial port. The GPS data message length is 76 bytes long. The message 
should have proper header and a valid checksum in order to be recognized as a valid 
message. 

The compass data is received over a RS-232 interface via the COM2 serial port. 
The message length for compass data is 60 bytes long. The TCM2 compass is configured 
to supply magnetic heading information. Magnetic declination [Ref. 18] is added to 
determine the true north. For Monterey Bay, this value is 15 degrees and 6 minutes [Ref. 
17]. It depends on location (latitude, longitude) and year. 

IMU data is received over a RS-232 interface via the COM3 serial port. The 
Crossbow IMU runs in VG mode. The IMU message has 22 bytes of data. The data 
packet consists of stabilized pitch and roll angles along with angular rate and linear 
acceleration information [Ref. 13]. 

The water speed sensor data is received through an A/D converter. The output of 
the sensor is voltage. 

The INS class implements the inertial navigation portion of the SANS. It is based 
on an asynchronous Kalman filter. The INS class instantiates a Sampler object from 
which it obtains heading, speed, linear acceleration data, and angular rate data. GPS 
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information is also passed to the INS class via the Navigator object. The INS produces 
accurate navigation information by integrating IMU data and DGPS data [Ref 13]. 

The Sampler prepares raw IMU, heading and water speed data for use by the INS 
object. The Sampler controls the data formatting and returns a formatted sample if valid 
raw data is available. Otherwise, a negative response is returned. 

The navigator object interfaces with both the GPS and INS .objects to determine if 
they have an updated estimate of the current position, and provides an estimate of the 
current position in hours, minutes, seconds and milliseconds of latitude and longitude. 
If GPS information is available, the navigator object converts a latitude and longitude 
expressed in milliseconds to a grid position in feet and passes it to INS class, so that the 
INS object can calculate the current position estimate with new GPS information. If no 
GPS information is available, the INS object calculates the current position estimate by 
using Kalman Filter equations. 

The software description for configuring the Sealevel C4-104 four RS-232 serial 
ports can be found in [Ref. 13]. 

D. SUMMARY 

The components in the current SANS hardware configuration were chosen based 
on size, cost, power, and ease of operation. The new components reduced the size of the 
system by 52% [Ref. 14]. The C4-104 Serial I/O Module provided four RS-232 serial 
ports for the PC/104 application. The various test results of the SANS III showed that the 
AMD 5S6DX133 based PC/104 computer provided more computing power and, more 
importantly, increased reliability and compatibility with PC/104 industrial standards. 


12 



The technological advances in sensors make it possible to change out the current 
components of the system for the newer versions in future years. With this feature, more 
accurate navigation information can be acquired, as well as smaller size advantages. 

All additions and updates to the SANS software were compiled under the Borland 
version 5.0, C 44 compiler. The software runs on a DOS (standard) platform with an 
AMD 586DX/133 MHz processor. 

Since DGPS information is available aperiodically due to asynchronous 
reacquisition time of satellite signals and asynchronous submergence and surfacing 
duration of the AUV, an asynchronous Kalman filter is needed to optimally integrate 
IMU and DGPS data. A Kalman filter was implemented in the INS object to improve the 
accuracy of SANS. 

Minor changes have been made in the SANS software. The changes were mostly 
related to the Kalman filter part of the system. The gains and constants were tuned to 
obtain improve accuracy. 

Most of the SANS software is still same as in Appendices A and B of [Ref. 13]. 
The modified part of the SANS software is presented in Appendix A. 
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III. SANS ASYNCHRONOUS KALMAN FILTER 


A. INTRODUCTION 

The objective of the SANS is to produce real time navigation information by 
integrating an Inertial Navigational System with a Differential Global Positioning System 
(DGPS). The navigation software in the previous versions of SANS was based on a 
twelve-state constant gain filter. Filter gains were initially selected based on bandwidth 
considerations and later tuned based on tilt table and bench testing. This filter is being 
replaced by an asynchronous Kalman filter in SANS III. This chapter discusses the 
developed discrete time asynchronous Kalman filter (Figure 3.1) that estimates the eight 
states of the SANS III model after attitude estimation. A brief discussion will be 
provided on Kalman filter basics leading into a derivation of the SANS III Kalman filter 
equations. - 

B. KALMAN FILTER BASICS 

The Kalman filter is a recursive predictive update technique used to estimate the 
states of a process which can be defined as a discrete-time or continuous-time model. For 
our purpose, we will study the discrete-time model of the Kalman filter. This state-space 
filter method has two main features (1) vector modeling of the random processes under 
consideration, and (2) recursive processing of the noisy measurement (input) data [Ref. 
2]. Given some initial estimates, it allows the states of a model to be predicted and 
adjusted with each new measurement. It also provides an estimate of error at each 
update! Kalman filters incorporate the effects of measurement noise and modeling noise 
in their computational structure. 
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Figure 3.1 SANS Navigation Filter [From Ref. 13] 
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We assume that the random process to be estimated can be modeled in the form : 

x * + i =0**+w* (3.1) 

In cases where the relationship between model and its measurements is linear, the 

observation (measurement) of the random process model (Eq. 3.1) becomes: 

z i =H yt x i + v A (3.2) 

Throughout the remainder of this chapter, various terms are usecT to better explain the 

Kalman filter. The following notations are consistent with [Ref. 2]: 

x k (nxl) Vector containing actual states of a process model at time t k . 
x k (nxl) Vector containing the current estimated states at t k . 
x~ (nxl) Vector containing the estimated states at time t before updating with 
measurement. 

£~ +] (nxl) Vector containing the estimated states at the next time sample. 

(n x n) Matrix containing the error covariance for x k ■ 
p~ (n x n) Matrix containing the error covariance for £~. 

P" +1 (n x n) Matrix containing the error covariance for x ~ M . 
z t (m x-1) Vector containing the measurement at time t k . 

H* (m x n) Matrix giving the ideal (noiseless) connection between the measurement 
and the state vector at time t k . 

R k (m x n) Matrix denoting the measurement error covariance, which must be known 
or estimated a priori. 

<j> k (n x n) Matrix containing the model relating x k and £~ +1 in the absence of a forcing 
function (if x* is a sample of a continuous process, <f> k is the usual state transition 
matrix). 

w k (nxl) Vector whose elements are white sequences with known covariance 
structure to be used with x k . 

\ k (m x 1) Vector whose elements are the measurement errors associated with z k - 
assumed to be a white sequence with known covariance structure and having zero 
cross-correlation with the w k sequence. 

Q k ' (tix n) Matrix containing the covariance of w,. 

K k (n x n) Kalman Gain matrix that relates the amount of influence that the error 
between i; and z k has in deriving x k . 

Tj time constant of a given state vector x* 
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q i white noise variable with zero mean and variance of element being modeled. 

D, zero frequency white noise density, (magnitude of q{) 

The basic discrete-time Kalman filter contains five recursive equations. 

Beginning with a prior estimate, the noisy measurement z k is used with a blending factor 

K^to improve the the estimate as follows; 

x k = £* + K k (z k - H^xj) ~ (3-3) 

K k (known as the Kalman gain) is now needed to find the optimal estimate x k . It 

takes the error covariance (mean-square error) between the current state x k and the 

estimated state x k and applies it with H* and R t resulting in 

K* =P;H[(H jt P;H[ + R*)' 1 (3.4) 

Once the Kalman gain K* minimizes the mean-square estimation error, a new 

covariance matrix can be computed for x k (Eq. 3.3) with 

P i =(I-K,H,)P; (3.5) 

The updated estimate x k is projected ahead using the state transition matrix <f > k . 

Unlike Equation 3.1 the noise vector w k does not affect the projected states because it 

has zero mean and is white (zero correlation with any previous w A ). Thus, 

** + i ( 3 - 6 ) 

Finally, the projected error covariance for x^ +] uses the updated error covariance 
P 4 from Equation 3.5, the covariance of w k in Equation 3.1, denoted as Q k , and <f> k the 
state transition matrix to form the following: 

I*A+1 =( f > lFk < t > k +Q* (3-7) 
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Equations 3.3 through 3.7 can be placed into an algorithm that can loop indefinitely. This 
Kalman filter loop is shown in Figure 3.2. 


Enter prior estimate x 0 and its error covariance P 0 



Figure 3.2 Kalman Filter Loop [From Ref. 2] 

C. DERIVATION OF SANS III KALMAN FILTER EQUATIONS 

The SANS III asynchronous Kalman filter has six states for orientation estimation 
(still constant gain), and eight states for position estimation. The part of the filter for 
orientation estimation remains the same as reported in [Ref. 11]. Figure 3.3 shows the 
process model developed by the SANS team [Ref. 11]. In this model, the velocity 
relative to water, ocean current, and GPS bias (state variables x, through x 6 ) are 
modeled as colored signals generated by white noises q l ,...,q 6 through first-order 
systems. The velocity relative to the ground is obtained by summing the velocity relative 
to watef (X[ and x 2 ) and the ocean current (x 3 and x 4 ). This result is integrated to 
obtain position estimation (x 7 and x g ). 
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Figure 3.3 SANS Process Model (after attitude estimation) [From Ref. 11] 

Low-frequency DGPS noise is explicitly modeled based on an experimentally 
obtained autocorrelation function. Ocean currents are modeled as a low-frequency 
random process. Finally, the asynchronous nature of the DGPS measurements resulting 
from AUV submergence or wave splash on the DGPS antenna is also taken into account 
by adopting an asynchronous Kalman Filter as the basis for the SANS position estimation 
software. 

Therefore, the Kalman filter state equations are characterized by: 


1 1 

— Xj H- 

Tj Tj 


(3.8) 


1 1 

-* 1+—?2 

*2 T 2 


(3.9) 
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(3-10) 


1 1 

*3 =-* 3 +— ?3 

T, T, 


1 1 

*4 =- x A +—q 4 

X 4 *4 


(3.11) 

1 1 

*5 =- x 5 +—q 5 

*5 ^5 


(3.12) 

1 1 

*6 =- x 6 +—q 6 

*6 *6 

■ 

(3.13) 

Xj = X, + x 3 


(3.14) 

x s =x 2 + x 4 


(3.15) 


X/ represents the time constant of the state vector x.. r, and t 2 are the speed 
through water time constants, r 3 and r 4 are the water current time constants, and 
r 5 and r 6 are the GPS time constants. 

Acceleration signals from the attitude estimation part of the SANS III filter were 
not used for velocity estimation because they were judged to be too noisy to provide 
useful information in comparison to values for velocity obtained directly from an 
accurate water speed sensor. With the process model shown in Figure 3.3, the 
measurements used for position estimation are the velocity relative to water provided by 
the water speed sensor and position information provided by DGPS. The velocity 
measurements are synchronous and available at every sampling time. DGPS information 
is asynchronous and is only available when the AUV is surfaced. The two synchronous 
measurement equations are: 

z l =x l +v i (3.16) 
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z 2 =x 2 + v 2 


(3.17) 


where v /5 i=l,2 are white noise. That is, it is assumed that the velocity measurements 
contain additive white noise. The two asynchronous measurement equations are: 

z 3 =x 7 +x 5 (3.18) 

z 4 = x 8 +x 6 (3.19) 

From these equations, cj)^ (state transition matrix) was developed. 

x, =——x,+ — q x (3.20) 

T, T, 

By taking Laplace transform of Equation 3.20: 

sX l (s)-x l (0 ) = -—x l (s) + —q l (s) 

X, X, 

(s + -)X 1 (s) = x i (0) + -q l (s) 

X, X, 

X\ (s)— —-j X] (0) +-^^(5) (3.21) 

(5 + —) ^(^ + —) 

T i h 

By taking inverse Laplace transform of Equation 3.21: 

-—t 2 •' — -(i-x) 

Xy(t) = e T ‘ x { (0)-\ - \e T ‘ q } (x)dx 

T i o 

So the discrete time model can be written as: 

x x (t k +l) = e Tl x x (t k ) + — \e z ' q } (x)dx, At = t k+l -t k 

- • T i z 

Similarly, the discrete time model for Equations 3.8 through 3.13 can be developed as: 


22 



—-A/ J r *+i (t k x -x) 

X .(t k +\) = e x ‘ Xi (t k ) + - \e x ‘ + i = 23,4,5,6 (3.22) 

X * 


X / 4 


Next, we need to develop the discrete time model for Equations 3.14 and 3.15. 


Taking the Laplace Transform of x 7 = x, + x 3 ; 




_ *i(0) , Ui(s) , x 3 (0) , ttj(s) 

_ _+ - —+ — _+ - — 

5 + — ^(sH-) S + — X 3 (,S +—) 


X,( S )= 1 -x,(0) + -^- + - U J .(l \ + + -±d£l (3.23) 

' 5(5 + —) T i s(s +—) 5f5 + —) T 3 s(s + —) 


Finding inverse Laplace transform of - 


1 


s(s + ~) 

T 


1_ Kt_ + K 2 _ K \( s+ t ) + K 2 s 


•s/s + -,) S 5 + — sf S + -,) 

X X T 


K, = -K 2 
K x - = 1 

X 


K x = x and K 2 = -x 


1 


x x 


1 

-t 


, 1 . s 1 

s(s + -) s + - 

T X 


x-xe T = xfl-e T ) 


(3.24) 


By using the Equation 3.24, we can take the inverse Laplace Transform of 
Equation 3.23: 
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-JL, 


(<~r) 


x 7 (t) = x 7 (0) + x l (l-e T * )x l (0)+ JYl-e Tl )<h(t)dx 


- 1 , 


-O-Tj 


+ x 3 (i - e T3 >3 ( 0 ,) + JYl - e t3 )q 3 (xM (3.25) 

o 

Likewise, solution for Equation 3.15 can be found as: 


x i (t) = x i (0) + x 2 (\-e T2 )x 2 (0)+ j(l-e X2 )q 2 (x)dx 


+ x A (l-e X *')x 4 (0) + Jfl -e^ )q 4 (x)dx (3.26) 


and the discrete time model for Equations 3.14 and 3.15 can be modeled by using 
Equations 3.25 and 3.26: 


A/ 


/• (^k +1 *) 

x 7 (t k+l ) = x 1 (t k ) + x l (l-e X ')x l (t k ) + \(l-e T ‘ )q x (i)dx 


* k+l -—(t k +i~t) 

+ x 3 (l-e X, )x 3 (t k ) + j(l-e 13 * )q 3 (x)dx (3.27) 


A/ 


x s (t k+] ) = x s (t k ) + x 2 (l-e X *)x 2 (t k )+ j(l-e X2 )q 2 (x)dx 


A/ 


-- - ^j 

+ x 4 (l-e Jn-e T4 + )qjx)dx (3.28) 


Therefore, the discrete process model of the system is given by 

*(t k+ i) = ® k x(t k ) + w(t k ) 
Where the state transition matrix has the form of 


(3.29) 
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And the discrete white noises are given by 


l f ~( l k+l~ x ) 

'i(h) = — j e Tl QiWdi, i=l,2,...,6 


0 

0 

0 

0 

0 

0 

0 

1 


1 _ L(t r . —r) **+1 

w i(h)= } 1-e T| q x (x)dx+ J 
h L J 4 


1-e X3 q 3 (x)dx 


(3.30) 
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It is noted that q,(t), i=l,2,3,4,5,6 are continuous independent white noise sources with 
zero mean and variance D i . Thus; 

E[q i (t)q J (x)]= 0 ,i*j 
E[q i (t)q i (x)\=D i b(t-x) 

Next, the covariance matrix Q k of w (t k ) is computed: 

Qk =EMt k )w T (t k )] 

By noting the expression of w,(t t ) from the previous page, Q k should have the 
following form: 

q n 0 0 0 0 0 q xl 0 

0 ?22 0 0 0 0 0 #28 

0 0 q 33 0 0 0 g 37 0 

0 0 0 q 44 0 0 0 q 4g 

^ 0 0 0 0 q S5 0 0 0 

0 0 0 0 0 q 66 0 0 

#71 0 q n 0 0 0 q 17 0 

_ 0 q S2 0 q 84 0 0 0 # gg _ 




-(4 + I“£) (4+1 _7 7) 

e T ' E[u^)u x (ji)]e r ' 
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Likewise; 


1 -r(4+i-l) --<4+i~7) 


1 r r v*a+i ** —v**+i f u 

— I I « ' « " ¥«-?» 


1/1 4 4 


_ ^1 






'i 4 


D -— 
2r, 


). i = 2,3.,6 

2r, 

Deriving equations for the q 17 and q iS : 
q 17 =E[w 1 (t k )w 1 (t k )] 


= E 


A (4+1 £) ” /• 

JO-e” >,«)<*#+ J(l-« 


-(4+1-#) 


f --( 4 + 1 - 7 ) '*>' ~('i + i- 7 ) 

J (1 - e )u l {q)dq+ J(l-e r ’ )u 3 (r])dq 


4+1 4+1 —(4+,-U --(4+1-7) 

f J ri ■-« Ti mu , <wi - * Ts 


+ 


*+! ‘*+1 

I Jo- 


—(4+1-5) 


(h+\-T\) 


)E[u 3 K)u 3 (T\)](\-e " )d^d n 


4 4 


4+i 

4+i / ~ 

2 

4+i 

4+i 

-a i 

4 

1 -e r ' 

di + A J 

4 

1 - e r ’ 
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4+i 

4+i “-5 2(4 +1 -£) 

4+1 

4+i 2(4 +1 -£) 

-A J 

1 -2e r ' +e r ’ 

rf# + A f 

1 - 2e r3 + e 

4 







#77 — A 



f -1^1 


f _2M> 
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< 2|A/p 


A? - 2t, 

l-e r ' 

+i 

l-e Tl 


+ Do 

At - 2t, 

l-e t3 


l-/~ 


1 


2 



j 

J 


2 



_ 

l J 


l y 

_ 



1 J 


V J 

_ 


Similarly; 

#88 = A 


A? - 2r, 


r _l*h 

l-e 12 


v y 


+ -*- 
• 2 


f JM\ 

l-e 12 
v J 


+ D, 


At - 2z. 


I*h 


l-e u 


V J 


l-e u 


For the terms q u and q 7l : 

qi 7 = q 71 = E[w, (t k )w 7 (f*)] 


= E 


1 '*>' --(4+1-#) 

-fe T ' «,(£>*£ 

4+i 

1 

f 4+i-7 ^ 

l-e r ' 

4+i 

u \0l)dij + J 

( 4+1-7 > 

l-e rj 

m 3 Oj)dtj ■ 
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l J 
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?28 = 482 = E l W 2 (h K (fk )h D 2 




?48 = ?84 = E [ W A (*k K (4 )]=^4 



and v* is measurement noise with covariance R 
".5 0 0 O' 

0 .5 0 0 ____ . . 

R = with DGPS signal 

0 0 0 0 
' _0 0 0 0 _ 

R = 5 ^ without DGPS signal 

_0 .5_ 

The H matrix is the noiseless connection between measurement and the state vector at 
time t. For the SANS asynchronous Kalman filter, two H matrices describe this 
connection, one for samples with DGPS the other for samples without DGPS. 

'1 0 0 0 0 0 0 O' 

0 1 0 0 0 0 0 0 ., __ . , 

H = with DGPS signal 

0 0 0 0 1 0 1 0 

0 0 0 0 0 1 0 1 

ri 0 0 0 0 0 0 01 . , 

H = without DGPS signal 

_0 1 0 0 0 0 0 0_ 
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The SANS Kalman filter Model that was developed above, was tested in 
MATLAB. The model was assumed stationary throughout the simulation. Simulation 
results of the SANS Kalman filter are presented in the following chapter. 

The MATLAB source code for the SANS Kalman filter is presented in Appendix 
B. 
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IV. SYSTEM CALIBRATION AND TESTING 


A. INTRODUCTION 

This chapter presents the calibration of system components, and the testing results 
of the current SANS configuration. After integrating the new hardware and 
implementing the new software, calibration of individual devices was required to achieve 
better results before testing the overall system. Bench testing was performed to 
determine the functionality and accuracy of the entire system. Ground vehicle testing, 
with the SANS system was mounted on a moving golf cart, was conducted to 
demonstrate the feasibility of the SANS system and to observe system performance 
before at-sea testing. These tests were also conducted to further tune the gains and 
constants of the SANS Kalman filter. 

Matlab simulation results and hardware bench testing results of the SANS 
Kalman filter are also presented in this chapter. 

B. CALIBRATION OF IMU AND COMPASS 

1. Tilt Table Calibration of IMU 

In order to calibrate the scale factors of accelerometers and determine K1 of the 
12-state complementary filter, the IMU unit was placed on a Haas rotary tilt table, model 
TRT-7 (Figure 4.1). The table has two degrees of freedom (DOF) and is capable of 
positioning to an accuracy of 0.001 degrees at rates ranging from 0.001 to 80 degrees/s 
[Ref. 16], 
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Tuning data for the IMU was obtained by moving the unit through each DOF at 
varying rates within a 45 degree range. The attitude as determined by the unit was then 
plotted and compared with the actual motion of the table. Through this comparison, it 
was possible to determine initial gain values and scale factors. The tilt table data was 
then postprocessed using these initial values and once again compared to the actual 
motion of the table. This process was repeated several times until the attitude determined 
by the filter “matched” the true motion of the table. 

Figure 4.2 shows an example of the results obtained during the timing process. 
The trajectory of the tilt table is not available to plot. Only the response of the IMU unit 
to 45 degree tilt table input is plotted. However, given the 10 degree per second pitch 
rate of the tilt table, it can be seen that the system accurately recorded each attitude 
change as taking 4.5 seconds to complete. The slight overshoots following each motion 
may indicate that the scale factor for the y- axis angular rate sensor is slightly high. This 
effect may also be due to undersampling problems. The flatness of the curve following 
stabilization after each motion indicates that a reasonable gain value has been determined 
as does the distance between the tails of each step [Ref. 11]. 

As exemplified by Figure 4.2, tilt table tests of the IMU unit show that attitude 
sensing is achieved to an accuracy of one degree or better under demanding 
circumstances. 

2. Magnetic Compass Calibration 

To-obtain the heading information in the SANS, the angular rate sensor is used as 
a high frequency input source, and the Precision Navigation Electronic Digital Compass 
(model TCM2) is used as low frequency data source. In a series of ground vehicle tests, 
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Figure 4.3 Compass Heading without Correction vs. DGPS Track 



heading(degrees) 


Figure 4.4 TCM2 Compass Error 
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with continuous GPS update (solid line). It can be seen, particularly during the north- 
south run, that the filter was not receiving accurate directional information. 

The investigation of compass error focused on three areas: possible interference 
produced by the golf cart electric motor, vibration, and error of the compass itself. 

The tests performed by Knapp [Ref. 15] showed that, golf cart electric motor 
caused interference, but its magnitude appeared to be limited to approximately half of a 
degree and could be compensated for with an appropriate value for the filter gain. It was 
also found that the vibration played a role in compass error, but still could be filtered out 
with an appropriate filter gain. 

A final test needed to determine the heading dependent compass errors, because 
of a different TCM2 was being used by SANS III. The TCM2 compass has a self¬ 
calibration [Ref. 3] routine which is designed to remove the effects of static magnetic 
fields caused by ferrous materials in the vicinity of the compass. The calibration routine 
is not capable of compensating for dynamic magnetic field distortions. From the ground 
vehicle testing results, the heading-dependent compass error can be seen. They are 
especially noticable on north-south runs. A transit (W. and L.E. Gurley) with an 
accuracy of 0.5 degrees was used as a reference to determine the error of TCM2 compass 
that was caused by the dynamic magnetic field of the golf cart. The TCM2 compass was 
mounted in line with the transit on the golf cart facing a distant object with a known 
magnetic bearing. By taking this object as a starting point, the compass was swung 
through, the entire 360 degrees, taking measurements every 10 degrees by using the 
opticaf scale of the transit. A comparison was made between the two indicated headings, 
the one from the optical scale of transit and the reading from TCM2 compass. Figure 4.4 
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optical scale of the transit. A comparison was made between the two indicated headings, 
the one from the optical scale of transit and the reading from TCM2 compass. Figure 4.4 
shows the difference between measurements of the transit and those of the TCM2 
compass. Using this data, a new lookup table and a linear interpolating function were 
added to the SANS code to compensate for heading-dependent errors. 

The major sensing component of the TCM2 compass is a, set of magnetic coils 
used to sense the magnetic field of the earth. These are not affected by acceleration or 
inertia as is the "card" in a mechanical compass. The TCM2 does use a "bubble sensor" 
to determine the attitude. This sensor is affected by linear acceleration to some degree. 
However, the only purpose of the TCM2 in the SANS system is to provide drift 
correction for the high frequency heading information obtained from the rate-sensors in 
the IMU. The magnitude and duration of any accelerations experienced in SANS system 
are relatively low and TCM2 data is low-pass filtered. Thus, the static accuracy of the 
TCM2 compass is investigated and not its dynamic accuracy in the presence of extended 
large magnitude accelerations. 

Figure 4.5 shows the data from the TCM2 after compensation using the table data. 
C. SYSTEM TESTING RESULTS 

1. Simulation and Hardware Bench Testing Results 
The asynchronous Kalman filter was simulated in Matlab, and implemented in 
SANS III. Simulation and hardware bench testing results are presented in this 
subsection. In simulation, asynchronism is coded as follows.. Every 20 to 30 
milliseconds, the filter takes measurements, to update the position estimates at the next 
(synchronous) sample time. 
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Figure 4.5 Compass Heading with Correction vs. DGPS Track 

In hardware testing, the system is fixed on a laboratory bench. The IMU, 
compass, and DGPS are operational. Water speed information is taken from a fixed 
voltage source in place of the water speed sensor. 

Simulation results are shown in Figures 4.6 and 4.7. The simulation is designed 
to simulate the hardware system in the stationary bench testing condition. Thus the 
actual position of the system is fixed at (0,0). It is seen in Figure 4.6 that the accuracy of 
position estimation is about 15 feet. In Figure 4.7, a north water speed of 10.0 
feet/second was entered. Since the system was stationary, the filter was able to estimate 
that there was a north current at about -10.0 feet/second as shown in Figure 4.7. The 
estimated east current stayed zero, as expected. 
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-15 -10 -5 0 5 10 15 20 

estimated east position(feet) 


Figure 4.6 Simulation Result: Estimated North Position 
Versus Estimated East Position 



time(seconds) 

Figure 4.7 Simulation Result: Estimated North (lower curve) 
and East Current (upper curve) 
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estimated east position(feet) 


Figure 4.8 Hardware Bench Test Result: Estimated North Position vs. 

Estimated East Position 



Figure 4.9 Hardware Bench Test Result: Estimated North (lower curve) 
And East Currents(upper curve) 
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Examples of hardware bench testing results are shown in Figures 4.8 and 4.9. In 
Figure 4.8, it is especially interesting to note that there is a long straightline segment 
starting from (0.2,0.0) ending at (0.0,10.0). This is exactly the estimated motion 
trajectory of the system during the first second of operation. During this time period, the 
filter relied entirely on inertial data. Since the water speed sensor indicated a water speed 
of 10.0 feet/second in the north direction, the filter estimated a northerly motion. As soon 
as a GPS fix was obtained, the filter updated position and current estimates. After about 
20 seconds, the estimated north current converges to 10.0 feet/second. 

Matlab simulation code for Kalman filtering is presented in Appendix B. 

2. Ground Vehicle Testing Results 

In addition to the bench testing, a series of ground vehicle tests were performed in 
preparation for at-sea testing. The SANS III system was placed on a golf cart (Figure 
4.10) and was driven along a predetermined path. Most of the tests were conducted 
around a surveyed course in the parking lot next to the Mechanical Engineering 
Auditorium at Naval Postgraduate School. Some additional tests were also performed in 
the parking lot at the Navy Golf Course to observe the behavior of the system in a 
different location. 

Tests were conducted with continuous DGPS and without DGPS. The path of the 
parking lot (at school) taken with continuous DGPS is shown in Figure 4.11. The cart 
started from the (0,0) position and traveled northward (at 350 degrees) for about 450 feet, 
made a-U--tum, traveled southward 248 feet (at 160 degrees) and about 210 feet (at 150 
degrees), made a last turn to westward and returned to the starting point along the last leg 
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Figure 4.11 Parking Lot Test Track (at School) 
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east/west runfeet) 


Figure 4.12 Parking Lot Track with Continuous DGPS vs. without DGPS 

(without compass correction) 



Figure 4.13 Parking Lot Track with Continuous DGPS vs. without DGPS 

(with compass correction) 








Figure 4.14 Speed Data Through First Runs 

newSpeed = n*measuredSpeed + (1 -n)*previousSpeed (4.2) 

previousSpeed = newSpeed (4.3) 

The speed value was also scaled, because the later test results showed that the calculated 
speed value from the circuit output was slightly higher than the actual value. 

These corrections improved the speed data significantly (Figure 4.15). 



Figure 4.15 Speed Data After Filtering 
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After compensating heading-dependent compass error and fixing the problems 
with speed sensor, other ground vehicle tests were performed. These results are depicted 
in Figures 4.16 and 4.17. They were taken at the Navy Golf Course parking lot. The plus 
signs represent the DGPS measurements of the vehicle trajectory. The solid line is the 
trajectory of the vehicle following the same path computed by the SANS III filter without 
using DGPS data during the entire run. Taking the DGPS data asihe reference, it is seen 
that the result of the SANS III without DGPS is accurate to within 15 feet throughout the 
run. 

D. OBSERVATIONS 

The following observations were made while conducting these tests. It is 
necessary to receive a proper DGPS signal to run the SANS software. If a valid 
differential correction is received about every five seconds, the SANS program will be 
able to be run in DGPS mode. It took a considerably long time to get a valid DGPS 
signal for ground vehicle testing. These are several things that could be the reason for 
not getting a proper signal. 

One of the reasons could be the differential antenna. It was replaced with a more 
powerful one to eliminate this possibility. 

The signal itself could have been the cause the problem. Two ground stations in 
Monterey Bay area are transmitting DGPS signals for our specially designed unit at a 
certain frequency. If the testing area is open to both stations, the differential unit 
sometimes receives both signals back to back which causes the program not to run in 
DGPS mode. The signal was tested with a hand receiver, and it was hypothesized that 
there is a synchronization problem between the ground stations. Most of the time, the 
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east/wsst run(feet) 


Figure 4.16 Continuous DGPS vs. without DGPS Track 
(without compass correction) 



east/west run(feet) 


Figure 4.17 Continuous DGPS vs. without DGPS Track 
(with compass correction) 
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signal was reasonably good and strong enough to use. 

Eliminating these two possible problems didn’t solve the problem with the DGPS 
signal. It turned out that when the computer of SANS was turned on, the DGPS signal 
went away. The SANS computer interfered with the DGPS signal. Everything on the 
golf cart was grounded to eliminate the noise causing interference and a metal shield for 
the SANS computer was fabricated to eliminate any frequency related interference. This 
helped, but didn't solve the problem completely. 

E. CONCLUSIONS AND SUMMARY 

The purpose of this chapter was to present the latest testing results of SANS III 
and explain the calibration procedures for the current hardware components. These 
results showed that the Crossbow IMU had an accuracy of one degree with proper gains 
and scale factors, and the TCM2, after heading dependent error correction, could supply 
information with an accuracy of one degree. 

The simulation and the hardware bench test results demonstrated that the SANS 
Kalman filter was working properly. The ground vehicle tests proved that the overall 
SANS III was able to navigate within ± 15 feet of Global Positioning track with no 
Global Positioning update for three minutes. 
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V. CONCLUSIONS 


A. SUMMARY 

The purpose of this thesis was to improve the navigation accuracy of the latest 
version of SANS by properly tuning Kalman filter gains. It also presents the test and 
calibration results. 

The research objectives of this thesis were: (1) to implement the asynchronous 
Kalman filter within the SANS system, (2) to tune filter gains by comparing the 
simulation and real hardware bench test results and, (3) to calibrate the components and 
test the overall system. 

The objective of the SANS system is to demonstrate the feasibility of using low- 
cost, small components to navigate inertially between DGPS fixes. To achieve the first 
research objective, the previous SANS constant-gain filter was replaced by an 
asynchronous Kalman filter, which has six states for orientation estimation (still constant 
gain), and eight states for position estimation. The asynchronous nature of DGPS 
measurements due to asynchronous reacquisition time of satellite signals and 
asynchronous submergence and surfacing of the AUV, made the selection of an 
asynchronous Kalman filter algorithm a logical choice. The results in Chapter 4 
confirmed that using a Kalman filter improved the accuracy of the system significantly. 

Bench test results indicated that the newly designed system provided a higher 
level of performance than the previous versions of SANS. Examination of the 
experimental data indicated that the new IMU used in this research was capable of 
meeting all SANS requirements. The new data acquisition and processing unit increased 
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the speed, reliability, and compatibility of the system. Testing the new asynchronous 
Kalman filter with different speed and heading data indicated that the new navigation 
filter worked properly. 

Ground vehicle tests demonstrated that the SANS III was able to navigate with an 
accuracy of ±15 feet. The current effort was directed toward at-sea testing. 

B. FUTURE RESEARCH 

The current hardware components seem to be working at a very reasonable 
sampling rate, but technology advances, software development, and the amount of 
research put into test and evaluation showed that the future of SANS will be subject to 
many changes. New SANS components should be chosen to reduce the size, while 
improving performance and decreasing cost. 

For the near future, the system is ready for at-sea testing. In order to acquire more 
accurate navigation information, the interference between the SANS computer and the 
DGPS unit should be solved. For future versions, it is suggested that the DGPS unit 
should be replaced with a new one which utilizes the broadcast Coast Guard signal. 

The performance of the new water speed sensor should be tested. The accuracy 
of the sensor seems acceptable for the SANS objective. It might need calibration prior 
to at-sea testing. 

The asynchronous Kalman filter has been implemented as a navigation filter. The 
filter gains were adjusted during the test and calibration period. While conducting at-sea 
testing, the filter constants and gains might need to be adjusted further. 
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During the ground vehicle testing, it appeared that magnetic compasses are very 
sensitive to environmental changes and hard to work with. Today’s laser gyros are 
getting smaller and cheaper. They are more robust than a magnetic compass. These 
should be considered for the future versions of SANS. 

Adding a local area network card to the system would improve SANS portability 
and could potentially change the way the sensors are integrated and utilized for 
applications other than AUVs. 
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APPENDIX A. MODIFIED PART OF NAVIGATION CODE(C++) 


A. INS.H 

#ifndef __INS_H 
#define _INS_H 
#include <time.h> 

#include <math.h> 

#include <dos.h> 

#include <stdio.h> 

#include <conio.h> _ 

♦include <fstream.h> 

♦include <iostream.h> 

♦include <assert.h> 

♦include "toetypes.h" 

♦include "globals.h” 

♦include "sampler.h" 

♦include "MatrixCls.h" 

/***************************************************************** 

CLASS: insClass 

AUTHOR: Eric Bachmann, Dave Gay, Kadir Akyol, Suat Arslan 

DATE: 11 July 1995 last modified March 2000 

FUNCTION: Takes in linear accelerations, angular rates, speed and 

heading information and uses Kalman filtering techniques to return a 
dead reconing position. 

**************************** ********* ****************** ***********/ 
class insClass { 
public: 

insClass(); // Constructor, initializes gains 

-insClass{) {} // destructor 

Boolean insPosition(stampedSample&); // returns ins estimated 
position 

// Updates the x, y and z of the vehicle posture 
void correctPosition(stampedSample&, double); 

// Sets posture to the origin and develops initial biases 
void insSetUp(double, stampedSampleS); 

private: 

file for filter data 
ofstream kalmanData; 

Matrix h, h_transpose, p, p_minus, rl, kl, k, x_hatMinus, x_hat, 
z, i, phi,phi_transpose, q, hi, hl__transpose, k2, r2, k3, z3, 
zMat; 
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7/ ins estimated posture (x y z phi theta psi) 
float posture[6] ; 

// ins estimated linear and angular velocities 
double velocities[6]; 

// time of last gps position fix 
float lastGPStime; 

// Accumulates deltaT between GPS data 
double gpstimeCounter; 

//accumulates deltaT 
double cumtime; 

// filter time constant 
int tau; 

samplerClass saml; // sampler instance 

matrix rotationMatrix; // body to euler transformation matrix 

float current[3]; // ins estimated error current 

// Software bias corrections for IMU rate sensors 
double biasCorrection[3]; 

// Complementary filter gains for orientation, 
float Konel, Kone2, Ktwo, speed; 

// Kalman Filter constants 

double tau__l, tau__2, tau_3, Dl, D2, D3; 

// Transforms body coords to earth coords, removes gravity comp, 
void transformAccels (doublet]); 

// Transforms water speed reading to x and y components 
void transformWaterSpeed (double, doublet]); 

// Tranforms body euler rates to earth euler rates, 
void transformBodyRates (doublet]); 

// Euler integrates the accelerations and updates the velocities 
void updateVelocities (stampedSample&); 

// Euler integrates the velocities and updates the posture 
void updatePosture (stampedSample&); 

// Builds the body to euler rate matrix 
matrix buildBodyRateMatrix(); 

7/ Builds the body to earth rotation matrix 
void buildRotationMatrix(); 

// Calculates the imu bias correction during set up 
void calculateBiasCorrections(stampedSample&); 
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// Applies bias corrections to a sample 
void applyBiasCorrections(stampedSample&); 

// Reads filter constants from 'ins.cfg' 
void readlnsConfigFile(); 

//constructs h(4*8) matrix 
void constructHmatrix(); 

//constructs P_minus(8*8) matrix 
void constructPminusMatrix() ; 

//constructs r(4*4) matrix _ 

void constructRlmatrix(); 

.//constructs h(2*8) matrix (h matrix without GPS) 
void constructHlmatrix(); 

//constructs r(2*2) matrix (r matrix without GPS) 
void constructR2matrix(); 

//constructs phi(8*8) matrix 

void constructPhiMatrix(stampedSample&); 

//constructs q(8*8) matrix 

void constructqMatrix(stampedSample&); 

} ; 

// Post multiply a matrix times a vector and return result, 
vector operator* (matrix&, doublet]); 

#endif 
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B. INS.CPP 

#include <iostream.h> 

#include <signal.h> 

#include <assert.h> 

#include <math.h> 

#include "ins.h" 

#include "matrix.h" 

#define SIGFPE 8 // Floating point exception 

/****************************************************** ************ 
PROGRAM: insClass (constructor) 

AUTHOR: Eric Bachmann, Dave Gay,Rick Roberts, Kadir Akyol, 

Suat Arslan 

DATE: 11 July 1995 last modified March 2000 

FUNCTION: Constructor initializes kalman filter gains and linear 
and angular velocities 
RETURNS: nothing 

CALLED BY: navigator class 
CALLS: none 

******************************************************************/ 
#ifndef _NO_NAMESPACE 
using namespace std; 
using namespace math; 

#define STD std 
#else 

#define STD 
#endif 

#ifndef _NO_TEMPLATE 
typedef matrix<double> Matrix; 

#else 

typedef matrix Matrix; 

#endif 

#ifndef _NO_EXCEPTION 

# define TRYBEGIN() try { 

# define CATCHERRORO } catch (const STD::exception e) { \ 

cerr « "Error: " << e.what() << 

endl; } 

#else 

# define TRYBEGIN() 

# define CATCHERRORO 
#endif 

void mylnverse(MatrixCls &); 

insClass::insClass():h("h matrix",4,8),h_transpose("h transpose", 8,4), 
p__minus("p minus", 8,8) , rl ( "rl matrix", 4,4) ,“ kl("kl", 4, 4), 
k("k matrix", 8, 4), x_hatMinus ("x_hatmin", 8, 1), 

x_hat("x hat”, 8,1), i("unit mat", 8, 8), 
phi_transpose("phitranspose", 8, 8), hi("hi",2,8), 
hl_transpose("hi transpose", 8, 2), r2("r2 matrix",2,2), 

k2("k2", 2, 2), k3("k mat no gps", 8, 2), 
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phi("phi matrix", 8, 8), q("q matrix", 8, 8), 
p("p matrix", 8, 8),z3("z3 matrix",2,1), 

zMat("zMat",4,1),kalmanData("xhat.dat"),gpstimeCounter(1.0) 

{ 

cerr << "\nconstructing insl" « endl; 


readlnsConfigFile(); // Read the config file 

constructHmatrix(); //constructs 4*8 h matrix 


constructPminusMatrix(); 


//constructs 8*8 P minus matrix 


constructRlmatrix(); 
constructHlmatrix(); 
constructR2matrix(); 


//constructs 4*J R1 matrix 

//constructs 2*8 h matrix 
//constructs 2*2 R2 matrix 


velocities[0] = 0.0; 
velocities[1] = 0.0; 
velocities[2] = 0.0; 
velocities[3] = 0.0; 
velocities[4] = 0.0; 
velocities[5] = 0.0; 


// x dot 
// y dot 
// z dot 
// phi dot 
// theta dot 
// psi dot 


} 


// Set posture to straight and level at the origin 


posture[0] = 0.0; 

// 

X 

posture[1] = 0.0; 

// 

y 

posture[2] = 0.0; 

// 

z 

posture[3] = 0.0; 

// 

phi 

posture[4] = 0.0; 

// 

theta 

posture[5] = 0.0; 

// 

psi 


cerr << "\nins construction complete" « endl; 


/********************************************* ********************* 
PROGRAM: insPosit 

AUTHOR: Eric Bachmann, Dave Gay, Kadir Akyol, Suat Arslan 

DATE: 11 July 1995 last modified March 2000 

FUNCTION:Make dead reckoning position estimation using kalman 
filtering.Inputs are linear accelerations, angular rates, speed and 
heading.Primary input data is obtained from a sampler object via the 
getSample method. This data is stored in the sample filed of a 
stampedSample structure called newSample. The sample field is then 
used as a working variable as the linear accelerations and angular 
rates it contains are converted to earth coordinates and integrated 
to determine current velocities and posture. The data is 
asynchronous kalman filtered against itself, speed and magnetic 
heading. 

RETURNS: position in grid coordinates as estimated by the INS 

CALLED BY: navPosit (nav.cpp) 

CALLS: getSample (sampler.cpp) 

findDeltaT (ins.cpp) 

transformBodyRates (ins.cpp) 
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buildRotationMatrix (ins.cpp) 
transformAccels (ins) 
transformWaterSpeed (ins) 




void fpelnsPosit(int sig) 

{if (sig — SIGFPE) cerr « "floating point error in insPosit\n";} 

Boolean insClass::insPosition(stampedSample& newSample) 

{ 

signal (SIGFPE, fpelnsPosit); 

// Working variables 

double thetaA, phiA, xlncline, ylncline; _ 

if (saml.getSample(newSample) ) { 

applyBiasCorrections(newSample); 

newSample.rawSample[0] = newSample.sample[0]; 
newSample.rawSample[1] = newSample.sample [1]; 
newSample.rawSample[2] = newSample.sample[2]; 
newSample.rawSample[3] = newSample.sample[3]; 
newSample.rawSample[4] = newSample.sample[4]; 
newSample.rawSample[5] = newSample.sample[5]; 
newSample.rawSample[6] = newSample.sample[6]; 

newSample.rawSample[7] = newSample.sample[7]; 

gpsTimeCounter+=newSample.deltaT; 
cumtime+=newSample.deltaT; 

xlncline = newSample.sample[0] / GRAVITY; 
ylncline = (newSample.sample[1] - 

(newSample.sample[5] * newSample.sample[6])) 

/ (GRAVITY * cos(posture[4])); 

if (fabs(ylncline) > 1.0 || fabs(xincline)>1.0) { 

static int inclineCount(0); 
gotoxy(1,24); 

cerr « "Inclination errors: " « ++inclineCount « endl; 
return FALSE; 

} 

// Calculate low freq pitch and roll 
thetaA = asin(xlncline); 
phiA = -asin(ylncline) ; 

// Transform body rates to euler rates, 
transformBodyRates(newSample.sample); 

-^--//-Calculate estimated roll rate (phi-dot). 

velocities[3] = newSample.sample[3] + Konel * (phiA - 
posture[3]); 

// Calculate estimated pitch rate (theta-dot), 
velocities[4] = newSample.sample[4] + Kone2 * (thetaA- 
posture[4]); 
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// Calculate estimated heading rate (psi-dot). 
velocities[5] = 

newSample.sample [ 5 ] + Ktwo * (newSample.sample[7] - 
posture [5]); 

// integrate estimated angular rates to obtain angles 
// pitch rate to angle 

posture[3] += newSample.deltaT * velocities[3]; 

// roll rate to angle 

posture[4] += newSample.deltaT * velocities[4]; 

// yaw rate to angle 

posture[5] += newSample.deltaT * velocities[5]; 

// constructs Phi matrix 
constructPhiMatrix(newSample); 

//constructs Q matrix (8*8) 
constructqMatrix(newSample); 

//calculate x_hatMinus 
x_hatMinus = ( phi * x__hat ) ; 

//calculate phi_transpose 
phi.transpose(phi_transpose); 

//calculate P_minus 

p_minus = ((( phi * p ) * phi_transpose ) + q ); 

// Beginning Kalman Filter loops 

if (newSample.gpsFlag &&’ gpsTimeCounter>=l.0) { 

zMat.copy(0,0,(newSample.sample[6] * cos (posture[5]))) 
zMat.copy(1,0,(newSample.sample[6] * sin (posture[5]))) 
zMat.copy(2,0,newSample.est.x); 
zMat.copy(3,0,newSample.est.y); 

//transpose of matrix h 

h.transpose(h_transpose); 

kl = (((h*p_minus)*h_transpose)+rl); 

//take inverse of matrix kl 
myinverse(kl) ; 

//calculate matrix Kalman gain 
k = ((p_minus * h_transpose)* kl) ; 

//calculate x_hat 

x__hat = ( .x__hatMinus + (k * (zMat - (h * x_hatMinus) ) ) ) 

'"//calculate I matrix 
i = i.unitMatrix (8); 

//calculate P matrix 
p = ( (i - (k * h)) * pjninus); 
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// Writing filter output to file 
kalmanData « cumtime « 1 1 

« newSample.gpsFlag << 1 1 

« x_hat.getElement(0,0) << f 1 

« x__hat. getElement (1, 0) « 1 ' 

<< x_hat.getElement(2,0) « 1 ' 

« x_hat.getElement(3,0) « ? 1 

« x_hat.getElement(4,0) « 1 1 

« x_hat.getElement(5,0) « 1 1 

« x_hat.getElement(6,0) « 1 1 

« x_hat.getElement(7,0) 

<< endl; 

newSample.gpsFlag = FALSE; 
gpsTimeCounter = 0.0; 


} - 
else { 

z3.copy(0,0, (newSample.sample[6] * cos (posture[5]))) 
z3.copy(1,0, (newSample.sample[6] * sin (posture[5]))) 

//hi is the h matrix without GPS 
hi.transpose(hl_transpose) ; 

k2 = (((hl*p_minus)*hl_transpose)+r2) ; 

// take inverse of matrix k2 
myinverse(k2); 

//kalman gain matrix without gps 
k3 = ((pjminus * hl_transpose)* k2) ; 

x_-hat = ( x_hatMinus + (k3 * (z3 - (hi * x_hatMinus) ) ) ) ; 

//calculate I matrix 
i = i.unitMatrix (8); 

//calculate P matrix 
p = ((i - (k3 * hi)) * p_minus); } 

// Writing filter'output to file 
kalmanData << cumtime << T T 

« newSample.gpsFlag « ' 1 

« x_hat.getElement(0,0} « * 1 

« x_hat.getElement(1,0) « ' ' 

« x_hat.getElement(2,0) « T ' 

« x_hat.getElement(3,0) « 1 ' 

<< x_hat.getElement(4,0) << 1 1 

« x_hat.getElement(5,0) << f T 

« x_hat.getElement(6,0) « 1 1 

« x_hat.getElement(7,0) 

- « endl; 

} 

// estimated north and east positions 
posture[0] = xjhat.getElement(6,0); 
posture[1] = x_hat.getElement(7,0); 
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// estimated current values 
newSample.sample[0] = posture[0] ; 

newSample.sample[1] = posture[1] ; 
newSample.sample[2] = posture[2] ; 
newSample.sample[3] = posture[3]; 
newSample.sample[4] = posture[4]; 
newSample.sample[5] = posture[5]; 
return TRUE; 

} 

else { 

return FALSE; // New IMU information was unavailable. 

} 

} 

/********** ******************************************************** 
PROGRAM: insSetUp 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Initializes the INS system. Sets the posture to 

the origin.Initializes the heading using magnetic compass 
information. Initializes the last GPS fix and last IMU information 
times. 

RETURNS: void 

CALLED BY: initializeNavigator (nav) 

CALLS: calculateBiasCorrections (ins) 

getSample (sampler) 
buildRotationMatrix (ins) 
transformWaterSpeed (ins) 

************************************************************'*'**'*'*■*'/ 
void fpel-nsSetUp (int sig) 

{if (sig =- SIGFPE) cerr « "floating point error in inSetUp\n";} 

void insClass::insSetUp(double originTime, stampedSample& posit) 

{ 

cerr << " Initializing INS." << endl; 
signal (SIGFPE, fpelnsSetUp); 

saml.initSampler (); // Initialize the sampler 

saml.getSample(posit); 

cerr « " X accel = " << posit.sample[0] 

« ", Y accel = " « posit.sample[1] 

« ", Z accel = " « posit.sample[2] « endl; 

calculateBiasCorrections(posit); // set imu biases 

posture[5] = posit.sample[7]; //set initial true heading 

buildRotationMatrix(); //set initial speed 

transformWaterSpeed(posit.sample[6], velocities); 

posit.current[0] = current[0]; 
posit.current[1] = current[1]; 
posit.current[2] = current[2]; 
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laStGPStime = originTime; 


// initialize times 


cerr « " INS initialization complete.” « endl; 

} 

/********************************************************** 

PROGRAM: transformAccels 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Transforms linear accelerations from body coordinates 

to earth coordinates and removes the gravity component in the z 
direction. _ . 

RETURNS: void 

CALLED BY: navPosit 

CALLS: none 

**********************************************************/ 
void insClass::transformAccels (double newSample[]) 

{ 

vector earthAccels; 


newSample[0] -= GRAVITY * sin(posture[4]) ; 

newSample[l] += GRAVITY * sin(posture[3]) * cos(posture[4]) ; 
newSample[2] += GRAVITY * cos(posture[3]) * cos(posture[4]); 

earthAccels = rotationMatrix * newSample; 

newSample[0] = earthAccels.element[0] ; 
newSample[1] = earthAccels.element[1] ; 
newSample[2] = earthAccels.element[2] ; 


/ 




PROGRAM: 

AUTHOR: 

DATE: 

FUNCTION: 

coordinates 


transformWaterSpeed 
Eric Bachmann, Dave Gay 
11 July 1995 

Transforms water speed into a vector in earth 
and returns them in the speedCorrection variable. 


RETURNS: void 

CALLED BY: navPosit 

CALLS: none 

******************************************************************/ 


void insClass::transformWaterSpeed (double waterSpeed, double 
speedCorrection[]) 

{ 

double water[3] = {waterSpeed, 0.0, 0.0}; 
vector waterVelocities = rotationMatrix * water; 

spe'erdCorrection [0] = waterVelocities.element [0] ; 
speedCorrection [1] = waterVelocities.element[1]; 
speedCorrection [2] = waterVelocities.element[2]; 

} 
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PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS: 
CALLED BY: 
CALLS: 


transformBodyRates 
Eric Bachmann, Dave Gay 
11 July 1995 

Tranforms body euler rates to earth euler rates 
none 

insPosit 

buildBodyRateMatrix 




void insClass::transformBodyRates (double newSample[]) 

{ 

matrix bodyRateMatrix = buildBodyRateMatrix( ); 
vector earthRates = bodyRateMatrix * & (newSample [3] 

newSample[3] = earthRates.element[0]; 
newSample[4] = earthRates.element[1]; 
newSample[5] = earthRates.element[2]; 

} 




PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS: 
CALLED BY: 
CALLS: 


buildBodyRateMatrix 
Eric Bachmann, Dave Gay 
11 July 1995 

Builds body to Euler rate translation matrix. 

rate translation matrix 

insPosit 

none 




matrix insClass::buildBodyRateMatrix() 

{ 

matrix rateTrans; 


float tth = tan(posture[4] ), 
sphi = sin(posture[3]), 

cphi = cos(posture[3]) , 
cth = cos(posture[4]) ; 


rateTrans.element[0][0] 
rateTrans.element[0][1] 
rateTrans.element[0][2] 
rateTrans.element[1][0] 
rateTrans.element[1][1] 
rateTrans.element[1][2] 
rateTrans.element[2][0] 
rateTrans.element[2][1] 
rateTrans.element[2][2] 


1 . 0 ; 

tth * sphi; 
tth * cphi; 
0 . 0 ; 
cphi ; 

-sphi; 

0 . 0 ; 

sphi / cth; 
cphi / cth; 


return rateTrans; 

PROGRAM: buildRotationMatrix 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 
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FUNCTION: 
RET-URNS: 
CALLED BY: 
CALLS':' 


Sets the body to earth coordinate rotation matrix. 

void 

insPosit, insSetUp 

none 

*************************************************/ 


void insClass::buildRotationMatrix() 


{ 

float spsi = sin(posture[5]), 
cpsi = cos(posture[5]), 
sth = sin(posture[4]), 
sphi = sin(posture[3]), 
cphi = cos(posture[3]), 
cth = cos(posture[4]); 

rotationMatrix.element[0][0] = 
rotationMatrix.element[0][1] - 
rotationMatrix.element[0][2] = 
rotationMatrix.element[1][0] = 
rotationMatrix.element[1][1] = 
rotationMatrix.element[1][2] = 
rotationMatrix.element[2][0] = 
rotationMatrix . element [2] [1] = 
rotationMatrix.element[2][2] = 

} 


cpsi * cth; 

(cpsi * sth * sphi) - (spsi * cphi); 
(cpsi * sth * cphi) + (spsi * sphi); 
spsi * cth; 

(cpsi * cphi) + (spsi * sth * sphi); 
(spsi * sth * cphi) - (cpsi * sphi); 
-sth; 

cth * sphi; 
cth * cphi; 


/****************************************************************** 
PROGRAM: postmultiplication operator * 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Post multiply a 3 X 3 matrix times a 3 X 1 vector and 

return the result 
RETURNS: 3X1 vector 

CALLED BY: 

CALLS: Nonel 

******************************************************************/ 


vector operator* (matrix^ transform, double state[]) 

{ 

vector result; 

for (int i = 0; i < 3; i++) { 

result.element[i] = 0.0; 
for (int j = 0; j < 3; j++) { 

result.element[i] += transform.element[i][j] * state[j]; 

} 

}" — 

return result; 

} 

/****************************************************************** 
PROGRAM: calculateBiasCorrections 
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AUTHOR: Eric Bachmann, 

DATE: 11 July 1995 

FUNCTION: Calculates the 

of imu readings. 

RETURNS: none 

CALLED BY: insSetup 

CALLS: none 


Dave Gay, Rick Roberts 

initial imu bias by averaging a number 






void fpeCalculateBiasCorrections(int sig) 

{if (sig == SIGFPE) cerr « "floating point error in 
CalculateBiasCorrections\n";} 


void insClass::calculateBiasCorrections(stampedSample& biasSample) 
signal (SIGFPE, fpeCalculateBiasCorrections); 
int biasNumber(tau/10); 


biasCorrection[0] = 0.0; // p roll rate 

biasCorrection [1] = 0.0; // q pitch rate 

biasCorrection[2] — 0.0; // r yaw rate 

for (int i = 0; i < biasNumber; i++) { 

// Find the average of the biasNumber packets 
while(!saml.getSample(biasSample)) {/* */}; ■ 

// roll-rate/b# 

biasCorrection [0] +— biasSample.sample[3]/biasNumber; 
//-pitch-rate/b# 

biasCorrection[1] += biasSample.sample[4]/biasNumber; 
// yaw-rate/b# 

biasCorrection [2] += biasSample.sample[5]/biasNumber; 


// set biasSample correction fields to new bias correction values 
// negative biasCorrection value is taken so biases are added to 
// sensor values 

biasSample . bias [ 0] = biasCorrection [ 0 ] — -(biasCorrection[0]); 
biasSample.bias[1] = biasCorrection[1] = -(biasCorrection[l]); 
biasSample.bias[2] = biasCorrection[2] = -(biasCorrection[2]); 


/★***★****************************************Vr*********^********* 

PROGRAM: applyBiasCorrections 

AUTHOR: Eric Bachmann, Dave Gay, Rick Roberts 

DATE: 11 July 1995 

FUNCTION: Applies updated bias corrections to a sample. 

RETURNS: void 

CALLED BY: insPosit 

CALLS: none 


void insClass::applyBiasCorrections(stampedSample& posit) 
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const float sampleWght(posit.deltaT/tau); 
const float biasWght(l - sampleWght); 

//Calculate updated bias values 

biasCorrection[0] = (biasWght * biasCorrection[0]) 

- (sampleWght * posit.sample [3]); 

biasCorrection[1] = (biasWght * biasCorrection[1]) 

- (sampleWght * posit.sample[4]); 

biasCorrection[2] = (biasWght * biasCorrection[2]) 

- (sampleWght * posit.sample [5]); 


//Apply the bias to the sample 
posit.sample[3] += biasCorrection[0]; 
posit.sample[4] += biasCorrection[1]; 

posit.sample [5] += biasCorrection[2] ; 

//Save the bias to the sample 
posit.bias[0] = biasCorrection[0]; 
posit.bias[1] — biasCorrection[1]; 
posit.bias[2] = biasCorrection[2] ; 


PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS: 


**************************************************** 
readlnsConfigFile 

Rick Roberts, Eric Bachmann, Suat Arslan 
02 Nov 96 last modified march 2000 
Reads filter constants from 1 ins-, cfg' 
void 


CALLED BY:ins class constructor 


CALLS: 


none 








void insClass::readlnsConfigFile() 

cerr << "Reading ins configuration file. 11 << endl; 
ifstream insCfgFile("ins.cfg ", ios::in). 


if (!insCfgFile) { 

cerr « "could not open ins 


configuration file!" << endl; 


} 

else { 

char comment[128]; 
insCfgFile 
» Konel » comment 

» Kone2 » comment 
» Ktwo » comment 
- - - - - >> tau 1 » comment 

» tau~2 » comment 
» tau_3 » comment 
» D1 » comment 
» D2 » comment 
» D3 » comment 
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>> tau >> comment 
» speed » comment 
» current [0] >> comment 
» current[1] » comment 
» current [2] » comment; 

cout << "\nKonel: rf << Konel << TT \nKone2: " « Kone2 
« "\nKtwo: ,f << Ktwo << "\ntau_l: " « tau_l 
« "\ntau_2: " « tau__2 « "\ntau_3: " « tau_3 
« M \nDl: " « D1 « "\nD2: 11 « D2 
« "\nD3: " << D3 « ,r \ntau: ff « tau 
« f, \nx Current: " « current [0] « f, \ny Current: " 

<< current [1] << "\nz Current: "« current [2] « endl; 

} 

ins.CfgFile. close ( ) ; 

} 

/**************************************************************** 
PROGRAM: constructHmatrix() 

AUTHOR: Kadir Akyol 

DATE: 01 March 1999 

FUNCTION: constructs h matrix 
RETURNS: none 

CALLED BY:ins class constructor 
CALLS: none 

*****************************************************************/ 


void fpeGonstructHmatrix(int sig) 

{if (sig == SIGFPE) cerr << "floating point error in 
constructHmatrix\n TI ; } 

void insClass::constructHmatrix() 

{ 

signal (SIGFPE, fpeconstructHmatrix) ; 

h.copy(0,0,1.0); 
h.copy(1,1,1.0); 
h.copy(2, 4,1.0) ; 
h.copy(2, 6,1.0); 
h.copy(3, 5,1.0) ; 
h.copy(3, 7, 1.0) ; 

return ; 

}//end constructHmatrix() 


/****************************************************************** 
PROGRAM: constructPminusMatrix() 

AUTHOR: Kadir Akyol, Suat Arslan 

DATE: 01 March 1999 last modified march 2000 

FUNCTION: constructs Pjninus matrix 

RETURNS: none 

CALLED BY: ins class constructor 
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CALLS: none 


void fpeconstructPminusMatrix(int sig) 

{if (sig == SIGFPE) cerr « "floating point error in 
constructPminusMatrix\n";} 


void insClass::constructPminusMatrix() 

{ 

signal (SIGFPE, fpeconstructPminusMatrix); 


p_minus.copy(0,0,0.1); 
pjminus.copy(1,1,0.1) ; 
p_minus.copy(2,2,0.1); 
p_minus.copy(3,3,0.1) ; 
p_minus.copy(4,4,0.15 ; 
pjninus.copy(5,5,0.1) ; 
p__minus.copy(6,6,0.1); 
p_minus.copy(7,7,0.1) ; 


return ; 

}//end constructPminusMatrix() 




PROGRAM: constructPMatrix() 
AUTHOR: Suat ARSLAN 

DATE: 28 May 1999 


FUNCTION: constructs P matrix 

RETURNS: none 

CALLED BY: ins 


CALLS:- None 


void fpeconstructPMatrix(int sig) 

{if (sig == SIGFPE) cerr « "floating point error in 
constructPMatrix\n";} 

void insClass::constructPMatrix() 

{ 

signal (SIGFPE, fpeconstructPMatrix); 

p.copy(0,0,0.5); 
p.copy(1, 1,0.5); 
p.copy(2,2,1.0); 
p.copy(3,3,1.0) ; 
p.copy(4,4,10.0); 
p.copy(5,5,10.0); 
p.copy(6,6,15.0); 
p.copy(7,7,15.0); 

return ; 

}//end constructPMatrix() 

/****************************************************************** 
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PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS: 
CALLED BY: 
CALLS: 


constructRlmatrix() 

Kadir Akyol, Suat Arslan 

01 March 1999 last modified march 2000 

constructs rl matrix 

none 

ins class constructor 

none 

***************************** *************** 




/ 


void fpeconstructRlmatrix(int sig) 

{if (sig == SIGFPE) cerr « "floating point error in 

constructRlmatrix\n";} 


void insClass::constructRlmatrix() 

signal (SIGFPE, fpeconstructRlmatrix); 


rl.copy(0,0,0.01); 
rl.copy(1,1,0.01); 
return ; 

}//end constructRlMatrix() 


/*********************************^ + ^^^^ + ^^^^^^ 
PROGRAM: constructHlmatrix() 

AUTHOR: Kadir Akyol 

DATE: 01 March 1999 

FUNCTION: constructs h matrix 

RETURNS: none 

CALLED BY: ins class constructor 
CALLS: None 


void fpeconstructHlmatrix(int sig) 

{if (sig == SIGFPE) cerr « "floating point 
constructHlmatrix\n";} 


error in 


void insClass::constructHlmatrix() 

{ 

signal (SIGFPE, fpeconstructHlmatrix); 

hi.copy(0,0,1.0); 
hi.copy(l,1,1.0); 
return ; 

}//end constructHlmatrix() 






PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS: 
CALLED BY: 
CALLS: 


constructR2matrix() 

Kadir Akyol, Suat Arslan 

01 March 1999 last modified march 2000 

constructs r2 matrix 

none 

ins class constructor 
None 
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void fpeconstructR2matrix(int sig) 

{if (sig == SIGFPE) cerr « "floating point error m 
constructR2matrix\n";} 

void insClass: : constructR2matrix () 

signal (SIGFPE, fpeconstructR2matrix); 

r2.copy(0,0,0.01); 
r2.copy(0,1,0.0); 
r2.copy(1,0,0.0); 
r2.copy(1,1,0.01); 

return ; 


}//end constructR2matrix() 


PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS: 
CALLED BY: 
CALLS: 




constructPhiMatrix() 

Kadir Akyol, Suat Arslan 

01 March 1999 last modified march 2000 

constructs phi matrix 

none 

insPosit 


None 

********* 






* 


/ 


void fpeconstructPhiMatrix(int sig) 

{if (sig == SIGFPE) cerr « "floating point error in 

cunstructPhiMatrix\n"; } 

void insClass ::constructPhiMatrix(stampedSampleS delta) 

signal (SIGFPE, fpeconstructPhiMatrix); 
double xx, yy, zz; 

xx = - (delta.deltaT)/tau_l; 
xx = exp(xx); 

yy = - (delta.deltaT)/tau_2; 
yy = exp(yy); 

zz = - (delta.deltaT)/tau_3; 
zz = exp(zz); 

phi.copy(0,0,xx); 

phi.copy(1,1,xx); 

phi.copy(2,2,yy); 

phi.copy(3,3,yy) ; 

phi.copy(4,4,zz) ; 

phi;copy(5,5,zz); 

phi.copy(6,0,((l-xx)*tau_l)); 

phi ."copy (6,2, ( (1-yy) *tau_2) ) ; 

phi!copy(7,1,((1-xx)*tau_l)); 

phi.copy(7,3,((1-yy)*tau_2)); 
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return ; 

}//end constructPhiMatrix() 






PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS: 
CALLED BY: 
CALLS: 


constructqMatrix{) 

Kadir Akyol, Suat Arslan 
01 March 1999 
constructs Q matrix 
none 
insPosit 
None 






void fpeconstructqMatrix(int sig) 

{if (sig == SIGFPE) cerr « "floating point error in 
cunstructqMatrix\n";} 

void insClass::constructqMatrix(stampedSampleS delt) 

signal (SIGFPE, fpeconstructqMatrix); 

double uu, ww, vv,zz,yy, Q661,Q662,Q60,Q62,Q71,Q73,Q66; 

uu = -(2.0 * delt.deltaT)/tau_l; 

uu = exp(uu) ; 

ww = -(2.0 * delt.deltaT)/tau_2; 

ww = exp(ww); 

w = -(2.0 * delt.deltaT)/tau_3; 

vv = exp(vv); 

zz = --(delt.deltaT)/tau_l; 
zz = exp(zz); 

yy = -(delt.deltaT)/tau_2; 
yy = exp(yy); 

Q661=D1*((tau_l/2)*(1-uu)-((2*tau_l)*(1-zz))+delt.deltaT); 
Q662=D2*((tau_2/2)*(1-ww)-((2*tau 2)*(1-yy))+(delt.deltaT)); 
Q66=Q661+Q662; 

Q60=D1*(0.5+(0.5*uu)-zz); 

Q62=D2*(0.5+(0.5*ww)-yy); 

Q71=Q60; 

Q73=Q62; 

q.copy(0,0,((uu)*(Dl/(2.0*tau_l)))); 
q.copy(l,1,((uu)*(Dl/(2.0*tau_l)))); 
q.copy(2,2, ( (ww) *(D2/(2.0*tau_l)))); 
q.copy(3,3,((ww)*(D2/(2.0*tau_l)))); 
q.copy ( 4 , 4 ,( (vv)*(D3/(2.0*tau_l)))); 
q-.copy (5, 5, ( (vv) * (D3/(2.0*tau_l) ) ) ) ; 
q.copy(6,6,(Q66)); 
q.copy(7,7/(Q66)); 
q.copy(0,6,(Q60)); 
q.copy(6,0, (Q60)); 
q.copy(2,6,(Q62)); 
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q.copy(6,2,(Q62)); 
q.cdpy(1,7,(Q71)); 
q.copy(7,1,(Q71)); 
q.copy(3,7,(Q73)); 
q.copy(7,3,(Q73)); 


return ; 


}//end constructqMatrix() 

/****************************************************************** 
PROGRAM: mylnverse(MatrixCls&) 

AUTHOR: Suat ARSLAN — 

DATE: 29 july 1999 

FUNCTION: invert the matrix 

RETURNS: none 

CALLED BY: ins 
CALLS: None 


******************************* 
void mylnverse(MatrixCls& kl) { 




/ 


Matrix k3; 

k3.SetSize(kl.getCol(), kl.getRow()); 
for (int i = 0; i < kl.getColO; i++) { 

for(int j =0; j < kl.getCol(); j++) { 

k3(i, j) = kl.getElement(i, j); 


} 


} 

k3 = !k3; 


for-(int i = 0; i < kl.getColO; i++) 
for (int j = 0; j < kl.getColO 
kl.copy(i, j, k3(i, j)); 

} 


} 

}// end of function 
// end ins.cpp 


j++) { 
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C. ATOD.H 

#define _ATOD_H 

#include <stdio.h> 
♦include <fstream.h> 
♦include <iostream.h> 
tinclude <dos.h> 


♦define ENABLED 1 
♦define DISABLED 0 

♦define INPUT 1 
♦define OUTPUT 0 

//♦define TRUE 1 
//♦define FALSE 0 

♦define STATUS_BYTE 0 
♦define START_CONVERSION 0 
♦define READ_DATA 1 
♦define DACJJPDATE 1 
♦define CLEAR_BOARD 2 
♦define SCAN_BURST_SLCT 3 
♦define CHANNEL_SLCT 5 
♦define GAINJ3LCT 5 
♦define IRQ_ENABLE 5 
♦define EXT_TRIG_ENABLE 5 
♦define PPI_A 4 
♦define PPI_B 5 
♦define PPI_C 6 
♦define PPI_CTRL 7 
♦define TIMER_A 8 
♦define TIMER_B 9 
♦define TIMER_C 10 
♦define TIMER_CTRL 11 
♦define DAC1_LSB 12 
♦define DAC1_MSB 13 
♦define DAC2_LSB 14 
♦define DAC2_MSB 15 
♦define CLEAR_IRQ_STAT 14 
♦define CLEAR_DMA_DONE 15 

♦define PPI_PORT_A 0 
♦define PPI_PORT_B 1 
♦define PPI_PORT_C 2 

♦define DISABLE_BOTH 0 
♦ define 'SCAN_ENABLE 1 
♦define BURST_ENABLE 2 

♦define SOURCE_AD_START 0 
♦define SOURCE_DMA_DONE 1 
♦define SOURCE TRIGGER 2 



#define SOURCE_PACER_CLOCK 3 
class atodClass{ 
public: 

atodClass() {} ; 

-atodClass(){}; 

void InitatodO; 

void InitializeBoardSettings(unsigned , float ) 

float DigitalToReal(int DigitalValue); 

void ResetBoard(void); 

void ClearBoard(void); 

void ClearlrqStat(void); 

void ClearDmaDone(void); 

void SetChannel(unsigned char); 

void SetGain(unsigned char); 

void SetIRQStatus(char) ; 

void SetExternalTrigger(unsigned char); 
void SetlnterruptSource(unsigned char); 
void ScanBurstEnable(unsigned char); 
void SetScanChannels(int); 
void StartConversion(void); 
int ConversionDone(); 
int ReadData(void); 

void ClockMode(unsigned char, unsigned char); 
void ClockDivisor(unsigned char, unsigned int); 
void SetUserClock(float); 

- void SetPacerClock(float); 

_//Boolean ClockDone(unsigned char); 
unsigned char ReadDigitallO(unsigned char); 
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void WriteDigitallO (unsigned char, unsigned char); 
void ConfigurelOPorts(unsigned char, unsigned char) 
void UpdateDAC(unsigned char, float); 

}; 

#endif 

//end atod.h 
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D . ATOD . CPP 

#include ."atod.h" 

unsigned BaseAddress; 

float VoltageRange, 

DACSlope, 

ConversionFactor, 

Baseline ; 

int DACOffset; 

void atodClass::Initatod() 

{ 

InitializeBoardSettings(768,10.0) ; 
ResetBoard(); 

ClearBoard(); 

SetChannel(1); 

SetExternalTrigger(DISABLED); 

SetGain(1); 




InitializeBoardSettings 

The InitializeBoardSettings procedure is used to set the Base address 
variable and calculate the conversion factor for converting between 
digital ' 

values and volts. Since the base address variable is not set until 
this 

procedure is called, make certain that you call this procedure before 
any others in this file. 

******** **************************************************************/ 


void atodClass::InitializeBoardSettings(unsigned BA, float Range) 

{ 

BaseAddress = BA; 

VoltageRange = Range; 

ConversionFactor = VoltageRange / 4096.0; 

Baseline = 0; 




The Function DigitalToReal converts a digitized value to a real world 
value. In these sample programs the conversion factor and baseline 
correspond to converting between digitized values and volts. 
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float atodClass::DigitalToReal(int DigitalValue) 

{ 

return(DigitalValue * ConversionFactor + Baseline); 

} 

/************** + + *********** + ****** + ** + ** + + ** + *-k*-k-k-k + ****** + -i' + + *** + ***± 


ResetBoard 

The ResetBoard procedure is used to reset the DM5406. The 8255 PPI is 
configured so that ports A and C are input and port B is output, a 
dummy A-D conversion is performed and then the clear board command is 
sent. _ 

**** + ****** + ****************** + **************** + *****■****■************■* i 


void atodClass::ResetBoard(void) 

{ 

outportb(BaseAddress + PPI_CTRL, 0x99); /* Set PPI Port B for 

output */ 

outportb(BaseAddress + PPI_B, 0); /* Channel 0, Gain 1, 

Ext Trig = 


outportb(BaseAddress 
outportb(BaseAddress 

outportb(BaseAddress 
conversion */ 

delay(5); 

outportb(BaseAddress 


+ SCAN_BURST_SLCT, 

+ CLEAR_BOARD, 0) ; 

+ START_CONVERSION, 0 

+ CLEAR BOARD, 0); 


/* Start a Dummy 


/* Any value will do */ 


Disabled, IRQ = Disabled */ 

0 ); 

); 


} 




ClearBoard 

The clear board procedure writes to the CLEAR BOARD port on the 
DM5406. 


*************±±-k'k-k-k'k-k±'k-k±-k-k-k'k-k-k'k±*-k*'k-k±-k-k-k-k-k*-k-k*±'k-k-i'**'k-k'k-k-k**'k'k'k-k-k-i<:-k-k'kl 

void atodClass::ClearBoard(void) 

{ 

outportb(BaseAddress + CLEAR_BOARD, 0); /* Any value will do */ 

} 

ClearIrqStatus 
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The ClearlrqStat procedure reads from the CLEAR_IRQ_STAT port on the 
DM5406'; 

**********************************************************************/ 


void atodClass::ClearlrqStat(void) 

{ 

inportb (BaseAddress + CLEAR_IRQ_STAT) ; 

} 

/********************************************************************** 

ClearDmaDone 

The ClearDmaDone procedure reads from the CLEAR_DMA_ONE port on the 
DM5406. 

**********************************************************************/ 
void atodClass::ClearDmaDone(void) 

{ 

inportb (BaseAddress + CLEAR_DMA_JDONE) ; 

} 

/******** ************************************************************** 
SetChannel 

The SetChannel procedure is used to set the channel bits, B0..B3, in 
the CHANNEL SELECT register (BA +5). Note how this procedure sets 
only 

the channel select bits; it does not change the other bits in this 
register. 

This is important because if you unintentionally clear the other bits 
it 

can cause unexpected behavior of the DM5406. 
**********************************************************************/ 


void atodClass::SetChannel(unsigned char ChannelNumber) 

{ 

unsigned char B; 


B = inportb (BaseAddress + CHANNEL___SLCT) ; 
B = B & 240; 

B = B I (ChannelNumber - 1); 
outportb(BaseAddress + CHANNEL_SLCT, B); 
} " --- ' 


/* read current byte */ 
/* clear BO - B3 */ 

/* set channel bits */ 
/* write new byte */ 




SetGain 
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The SetGain procedure is used to set the Gain bits on the DM5406. 

Note "how this procedure sets only the channel select bits; it does not 
change the other bits in this register. This is important because if 
you unintentionally clear the other bits it can cause unexpected 
behavior 
of the DM5406. 

**** + ************** + ************ + **** + **************■*■***■*:*************/ 


void atodClass::SetGain(unsigned 


{ 

unsigned char Bi¬ 
switch (Gain) 

{ 

case 1 : Gain = 0; 
case 2 : Gain = 1; 
case 4 : Gain - 2; 
case 8 : Gain = 3; 
default : Gain = 0; 

} 


char Gain) 


break; 

break; 

break; 

break; 

break; 


B = inportb(BaseAddress + GAIN_SLCT); 
B = B & 207; 

B = B | (Gain * 16); 

outportb(BaseAddress + GAIN_SLCT, B) ; 

} 

/************************************* 


/* read current byte */ 
/* clear B4,B5*/ 

/* set gain bits */ 

/* write new byte */ 




SetIRQStatus 

The SetIRQStatus procedure is used to set or clear the IRQ Enable bit 
on the DM5406. A value of 1 passed to this procedure enables 
interrupts 

a value of 0 disables interrupts. 


void atodClass::SetIRQStatus(char IRQStatus) 

{ 

unsigned char B; 

B = inportb(BaseAddress + IRQ_ENABLE); 

B = B & 127; 

B = B I IRQStatus * 128; 
outportb(BaseAddress + IRQ_ENABLE, B); 

} 

j * * *•**;*:* *. **^********************************************************* 


/* read current byte */ 
/* clear B5 */ 

/* set IRQ select bit */ 
/* write new byte */ 


SetExternalTrigger 

The SetExternalTrigger routine is used to set the external trigger bit 
on the DM5406. A value of 1 passed to this procedure enables the 
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external trigger, a value of 0 disables the external trigger, 
★★★★★★★★★★★a**********************************************************/ 

void atodClass::SetExternalTrigger(unsigned char TriggerStatus) 

{ 

unsigned char B; 

B = inportb (BaseAddress + EXT__TRIG_ENABLE) ; /* read current byte */ 

B = B & 191; /* clear B6 */ 

B = B | TriggerStatus * 64; /* set Trigger bit */ 

outportb(BaseAddress + EXT_TRIG_ENABLE, B) ; /* write new byte */ 

} 

/*++*++**+*******+*++***+**************+******************************* 
SetInterruptSource 

The SetlnterruptSource procedure determines the source used for 
generating 

interrupts for the DM5406. Passing SOURCE_AD_START selects the A/D 
start 

convert; passing SOURCE_DMA_DONE selects DMA done; passing 
SOURCE_TRIGGER 

selects the trigger; finally, passing SOURCE_PACER_CLOCK 
to this procedure selects the pacer clock. 

**********************************************************************/ 

void atodClass::SetlnterruptSource(unsigned char Source) 

{ 

unsigned char B; 

B = inportb(BaseAddress + SCAN_BURST_SLCT); /*read current 

byte*/ 

B = B & 207; /*clear bits 4 and 

5*/ 

B = B | Source * 16; /*set new bits*/ 

outportb(BaseAddress + SCAN_BURST_SLCT, B); /*write new byte*/ 

} 

/********************************************************************** 
ScanBurstEnable 

The ScanBurstEnable procedure allows the user to enable either scan 
mode 

or burst mode. A value of 0 passed to this procedure disables both 
modes- 

a value of 1 enables scan mode; a value of 2 enables burst mode. 

* + + *****************************************************************/ 
void atodClass::ScanBurstEnable(unsigned char Enable) 
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{ 

unsigned char B; 


switch (Enable) 

{ 

case DISABLE_BOTH: Enable = 0; break; 
case SCAN_ENABLE: Enable == 2; break; 
case BURST_ENABLE: Enable = 3; break; 
default: Enable = 0; 

} 

B = inportb (BaseAddress + SCAN_BURST__SLCT) ; 
B = B & 63; 

B = B I Enable * 64; 

outportb(BaseAddress + SCAN__BURST_SLCT, B) ; 


/*read current byte*/ 
/*clea£ bits 6 and 7*/ 
/*set new bits*/ 
/*write new byte*/ 




SetScanChannels 

The SetScanChannels procedure determines how many channels are scanned 
or 

bursted while in the appropriate mode, selected using ScanBurstEnable. 
The parameter passed in this procedure is simply the number of 
channels the 
user wishes to include. 

*****★****************************************************************/ 
void atodClass::SetScanChannels(int NumChannels) 

{ 

unsigned char B; 

B = inportb(BaseAddress + SCAN_BURST_SLCT); /*read current 

byte*/ 

B ~ B & 240; /*clear bits 0 

through 3*/ 

B = B | (NumChannels - 1); /*set new bits*/ 

outportb(BaseAddress + SCAN_BURST_SLCT, B); /*write new byte*/ 

} 

/******************************************★*************************** 


StartConversion 

The StartConversion procedure is used to start conversions. 
**********************************************************************/ 


void atodClass::StartConversion(void) 

{ 
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outportb(BaseAddress + START_CONVERSION, 0) ; 

} 

/********************************************************************** 

ConversionDone 

The ConversionDone function returns TRUE if a conversion is complete, 
FALSE 

if a conversion is in progress. 

int atodClass::ConversionDone() 

{ 

unsigned char Status; 

Status = inportb(BaseAddress + STATUS_BYTE); /* read board status 

*/ 

if ( (Status & 1) == 1) 

return 1; /* if BO is set return TRUE */ 

else 

return 0; /* if BO is not set return FALSE */ 




ReadData 

The ReadData function retrieves two bytes from the converter and 
combines - 

them into an integer value. 




int atodClass::ReadData(void) 

{ 

unsigned char MSB, LSB; 
int DigitalValue; 

MSB = inportb(BaseAddress + READ_JDATA) ; 
LSB - inportb(BaseAddress + READ_DATA) ; 

DigitalValue = (MSB * 256 + LSB) ; 

return(DigitalValue) ; 

} 


ClockMode 

The ClockMode procedure is used to set the mode of a designated 
counter 

on the 8254 programmable interval timer (PIT). 
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void atodClass::ClockMode(unsigned char Clock, unsigned char Mode) 

{ 

unsigned char StatusByte; 

StatusByte = (Clock * 64) + (Mode * 2) + 48; 
outportb(BaseAddress + TIMER_CTRL, StatusByte); 

} 


/********************************************************************** 

ClockDivisor ~ 

The ClockDivisor procedure is used to set the divisor of a designated 
counter on the 8254 programmable interval timer (PIT). This procedure 
assumes that the counter has already been set to receive the least 
significant byte (LSB) of the divisor followed by the most significant 
byte (MSB). 


**********************************************************************/ 

void atodClass::ClockDivisor(unsigned char Clock, unsigned int Divisor) 

{ 

unsigned char MSB, LSB; 
unsigned int PortID; 

PortID = BaseAddress + TIMER_A + Clock; 

LSB = Divisor % 256; 

MSB = Divisor / 256; 
outportb(PortID, LSB); 
outportb(PortID, MSB); 

} 

/*********************************************** *********************** 
SetUserClock 


The SetUserClock procedure is used to set the programmable interval 
timer (PIT) on the DM5406 such that the output of counter 2 goes high 
at the specified rate. The maximum attainable rate this procedure, as 
written, is 250,000 Hz although you can easily change this by 
adjusting 

the divisors accordingly. 




void atodClass::SetUserClock(float Rate) 
{ ' -- 

ClockMode(0, 2); 

ClockDivisor (0, 8); 


ClockMode(1, 2); 

ClockDivisor(1, (500000.0 / Rate)); 



Cloc'kMode (2, 2); 
ClockDivisor (2, 2); 




SetPacerClock 

The SetPacerClock procedure is used to set the programmable interval 
timer (PIT) on the DM5406 such that the output of counter 1 goes high 
at the specified rate. The maximum attainable rate this procedure, as 
written, is 250,000 Hz although you can easily change^this by 
adj usting 

the divisors accordingly. 

Note that the Pacer and User clocks are really the same device and 
cannot be used independently.' 

**********************************************************************/ 
void atodClass::SetPacerClock(float Rate) 


ClockMode(0, 2); 

ClockDivisor(0, 16) ; 

ClockMode(l, 2); 

ClockDivisor(1, (500000.0 / Rate)); 


^********************************************************************** 
Read DigitallO 

The ReadDigitallO function returns the value of the specified digital 
input port. Each digital input line is represented by a bit of the 
return value. Digital in 0 is bit 0, digital in 1 is bit 1, and so on. 

**********************************************************************/ 


unsigned char atodClass::ReadDigitallO(unsigned char InputPort) 

{ 

return(inportb(BaseAddress + PPI_A + InputPort)); 




WribeBigitallO 

The WriteDigital10 function sets the value of the digital output port 
to 

equal the value passed as parameter v. Each digital output line is 
represented by a bit of v. Digital out 0 is bit 0, digital but 1 is 
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bit 1, 
and so on. 


void atodClass::WriteDigitallO (unsigned char OutputPort, unsigned char 
v) 

{ 

outportb(BaseAddress + PPI_A + OutputPort, v); 

} 


ConfigureIOPorts 

The ConfigureIOPorts procedure is used to configure the ports A and C 
on 

the 8255 PPI for either input or output. A value of 1 means input, a 
value 

of 0 is for output. It is advisable to use the INPUT and OUTPUT 
constants 

defined in this file. 

Port B remains set for output. 


**********************************************************************^ 

void atodClass::ConfigurelOPorts (unsigned char PortA, unsigned char 
PortC) 

{ 

unsigned char ControlByte; 

ControlByte = 128 + (PortA * 16) + (PortC * 9); 
outportb(BaseAddress+PPI_CTRL, ControlByte); 

} 

/**************************** ****************************************** 
UpdateDAC 

The UpdateDAC procedure outputs the specified voltage to the specifed 
DAC. The DACSlope and DACOffset variables must be set to the values 
required for the output range of the DACs. 

*********************************************** ***********************/ 


void atodClass::UpdateDAC(unsigned char DAC, float Volts) 

{ 

int Value; 

Value = (int) (Volts * DACSlope) + DACOffset; 

outportb(BaseAddress + DAC1_LSB + (DAC - 1) * 2, Value-% 256); 
outportb (BaseAddress + DAC1__MSB + (DAC - 1) * 2, Value / 256); 
outportb(BaseAddress + DAC_UPDATE, 0); 

} 

//end atod.cpp 
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APPENDIX B. MATLAB CODE FOR SANS KALMAN FILTER 

clear all 
format long 

gp s_f1ag_time=1; 
cum_time=0; 
x_hat=zeros(8,1); 

simulation_time=5.0; % in minutes 

samples=(simulation_time * 60) / 0.025; % 0.025 is the average delta_t 

heading=6.2832; % for north 

speed=10; 

x_hat_plot=zeros (samples, 9) ; ~ ' 

temp=zeros(1,9); 

% Time Constants 

tau_l = 10; % seconds for velocity 

tau_2 - 3600; % seconds for current 

tau_3 = 60; % seconds for GPS 

%R1 matrix 

rl=[.01 0 0 0 ; 0 .01 0 0 
0000; 0000]; 

%R2 matrix 

r2=[.01 0 ; 0 .01]; 

% P_minus matrix 

p_minus =[.l 0 0 0 0 0 0 0; 

0.1000000 ; 

00.100000 ; 

000.10000 ; 

0000.1000 ; 

00000.100 ; 

000000.10 ; 

0 0 0 0 0 0 0.1]; 

% P matrix 

p = [. 5 0 0 0 0 0 0 0 ; 

0.5000000 ; 

0 01 . 0 0 0 0 0 0 ; 

0 0 01 . 0 0 0 0 0 ; 

0 0 0 010.0 0 0 0; 

0 0 0 0 010.0 0 0; 

0 0 0 0 0 015.0 0; 

0 0 0 0 0 0 0 15.0]; 


%H1 matrix 

hl=.[ 1 JD 0 0 0 0 0 0 ; 
01000000 ; 
0G001010; 
Ch 0 000 101]; 
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%H2 matrix 

h2=[l 0000000; 

0 1 O' 0 0 0 0 0] ; 

D1=0.01; 

D2=0.01; 

D3=0.5; 

Dl_p = 0.01; 

D2_p = 0.01; 

D3_p = 0.5; 

% initial process state 

x_previous = [0; 0; 0; 0; 0; 0; 0; 0] ; 

%for plotting only. 
gps_plot = [ 0; 0]; 

'beginning kalman filter loops' 

for i=l:samples 

delta_t=0.02; % 0.01*rand 

gps__f lag_time=gps_flag_time+delta_t ; 

cum_t ime=cum__time+delta__t ; 

% construct Phi matrix 
uu=exp(“delta_t/tau_l); 
vv=exp(-delta_t/tau_2); 
ww=exp (-delta_t/tau__3) ; 

phi =[uu0000000; 

0 uu 0 0 0 0 0 0 ; 

00 vv 00000; 

0 0 0 vv 0 0 0 0 ; 

0000 ww 000; 

00000 ww 00; 

tau_l*(l-uu) 0 tau_2*{l-vv) 00010; 

0 tau_l*(l~uu) 0 tau_2*(1-vv) 0001]; 

%construct Q matrix for Kalman filter 
xx=exp((-2*delta_t)/tau_l); 
yy=exp((-2*delta_t)/tau_2); 
zz=exp((-2*delta_t)/tau_3); 

Q00=((Dl/(2*tau_l))*(xx)); 

Ql1=((Dl/(2*tau_l))*(xx)); 

Q22=((D2/(2*tau_l))*(yy)); 

Q33=((D2/(2*tau_l))*(yy)); 

Q4 4=((D3/(2*tauJL))*{zz)); 

Q55—(-(D3/ (2*tau_l) ) * (zz) ) ; 

Q661=D1*(delta_t-2*tau_l*(1-uu)+(tau_l/2)*(1-xx)); 
Q662=D2* (delta_t-2*tau_2* (1-w) + (tau_2/2) * (1-yy) ) ; 
Q66=Q661+Q662; 

Q77=D1*(delta_t-2*tau_l*(1-uu)+(tau_l/2)*(1-xx))+D2 
*(delta_t-2*tau_2*(1-vv)+(tau_2/2)*(1-yy)); 
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Q06=D1*((1/2)-(uu)+(1/2)*xx); 
Q60=Q06; 


Q2 6=D2*((l/2)-(vv)+(l/2)*yy); 
Q62=Q26; 

Q17=Q06; 

Q71=Q17; 

Q37==Q62; 

Q7 3=Q37; 


%Q matrix 

q - [QOO 00000 
0 Qll 0 0 0 0 
0 0 Q22 0 0 0 
0 0 0 Q33 0 0 
0 0 0 0 Q44 0 
0 0 0 0 0 Q55 
Q60 0 Q62 0 0 
0 Q71 0 Q73 0 


Q06 0 ; 

0 Q17 ; 
Q26 0 ; 

0 Q37 ; 

0 0 ; 

0 0 ; 

0 Q66 0; 

0 0 Q77] ; 


%construct Q matrix for process model 
xx=exp ( (-2*delta_t) /tau_l) ; 
yy=exp((-2*delta_t)/tau_2); 
zz=exp((-2*delta_t)/tau_3) ; 

Q00=((Dl_p/(2*tau_l))*(xx)); 

Qll=((Dljp/(2*tau_l))*(xx)); 

Q22=((D2_p/(2*tau_l))*(yy)); 

Q33=((D2_p/(2*tau_l))*(yy)); 

Q4 4=((D3_p/(2*tau_l))* (zz)); 

Q55-((D3_p/(2*tau_l))*(zz)); 

Q66l=Dl_p*(delta_t-2*tau_l*(1-uu)+(tau_l/2)*(1-xx)); 
Q662=D2_p* (delta_t-2*tau_2* (1-vv) + (tau_2/2) * (1-yy) ) ; 
Q66=Q661+Q6 62; 

Q77-Dl_p*(delta_t-2*tau_l*(1-uu)+(tau_l/2)*(1-xx))+D2_p 
*(delta_t-2* tau_2*(1-vv)+(tau_2/2)*(1-yy)); 
Q06=Dl_p*((1/2)-(uu)+(1/2)*xx); 

Q60-Q06; 

Q26=D2_p* ( (l/2)-(w) + (l/2)*yy) ; 

Q62=Q26; 

Q17=Q06; 

Q71=Q17; 

Q37=Q62; 

Q73=Q37; 


q_process 


[QOO 00000 
0 Qll 0 0 0 0 
0 0 Q22 0 0 0 
0 0 0 Q33 0 0 
0 0 0 0 Q44 0 
0 0 0 0 0 Q55 
Q60 0 Q62 0 0 
0 Q71 0 Q73 0 


Q06 0 ; 

0 Q17 ; 

Q2 6 0 ; 

0 Q37 ; 

0 0 ; 

0 0 ; 

0 Q66 0; 

0 0 Q77]; 


% process model noise 
white__noise = randn(8,l); 
w - sqrtm (q_process) * white__noise; 

w(l)=w(l)+0.02; % input to generate some speed 
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x = phi * x_previous + w; 
x_previous = x; 

% measurement noise from water speed sensor 
v - 0.2*randn(2,1) ; 

% those two equations are here for smoothing rather than prediction. 
x_hat_minus=phi*x_hat; 
p_minus= (phi*p*phi')+q; 

tmp (1) = cum_t ime ; 

if (gps_flag_time>=l) 

z_with_gps = hi * x; %measurement with GPS 
,z_with_gps (1) = z_with_gps (1) + v(l); 
z_with_gps(2) = z_with_gps (2) + v(2); 

z_with_gps(3) = 0.0 + x(5); 
z_with_gps (4) = 0.0 + x(6); 

% k 1 =p__minus*h 1 1 *inv { (hl*p_minus) *hl 1 +rl) ; 
kinv=inv((hl*p_minus)*hl T +rl); 
kl=p_minus*hl 1 *kinv; 

ii=ii+4; 

x_hat=x_hat__minus+kl*(z_with_gps-(hl*xjiat_minus)); 
p=(eye(8)-kl*hl)*p_minus; 

gps_plot(1) = z_jwith_gps(3); 
gps_plot(2) = z_with_gps(4); 

% for printing purpose 
for j —1:4 

tmp(j +1) = z_with_gps(j ) ; 

end 

gps_flag_time=0; 
else 

z no_gps = h2 * x; % measurement without GPS 

z__no_gps = z_no__gps + v; 

% k2=p_minus*h2'*inv((h2*p_minus)*h2'+r2); 
k2inv=inv((h2*p_minus)*h2 1 +r2); 
k2=p_minus*h2 1 *k2inv; 
k2invtmp=[k2inv(1,1) k2inv(l,2) 0 0; 

k2inv(2,l) k2inv(2,2) 0 0]; 

x_hat=x_hat_minus+k2*(z_no_gps-(h2*x_hat_minus)); 

_p=(eye(8)-k2*h2)*p_minus; 
for j =4:5 

tmp(j) = gps_plot(j-3); 

end 
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for j~2:3 

tmp(j) = z_no_gps (j-1); 

end 

end %end if statement 

% collecting measurement for plotting purpose 
zjolot(i, :) = tmp; 

% collecting x_hat for plotting purpose 
temp2(1)=cum_time; 
temp3(1)=cum_time; 
for j =2:9 

temp2 (j ) =x_hat (j-1 ) ; 
temp3(j)=x(j-1); 

end 

x_hat_plot (i, : ) =temp2; 
x_plot(i,:) = temp3; 

end %end of Kalman Filter loop 
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